/*
 * 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.
 */

#define _USE_MATH_DEFINES

#include "testModels.h"

#include <cmath>
#include <iCub/iDynTree/TorqueEstimationTree.h>

#include <iCub/iDyn/iDyn.h>
#include <iCub/iDyn/iDynBody.h>

#include <kdl/frames.hpp>

#include <yarp/os/Random.h>
#include <yarp/os/Time.h>

#include <yarp/math/api.h>
#include <yarp/os/Log.h>

#include <iCub/ctrl/math.h>
#include <iCub/skinDynLib/dynContactList.h>
#include <kdl_codyco/regressors/dirl_utils.hpp>

#include <iDynTree/Model/Model.h>
#include <iDynTree/Model/SubModel.h>
#include <iDynTree/Model/ModelTransformers.h>
#include <iDynTree/Model/ForwardKinematics.h>
#include <iDynTree/Model/ContactWrench.h>
#include <iDynTree/Model/Dynamics.h>
#include <iDynTree/Model/Traversal.h>
#include <iDynTree/Model/LinkState.h>
#include <iDynTree/Model/JointState.h>
#include <iDynTree/Model/FreeFloatingState.h>

#include <iDynTree/Core/EigenHelpers.h>
#include <iDynTree/Core/TestUtils.h>
#include <iDynTree/Core/ClassicalAcc.h>

#include <iDynTree/Sensors/Sensors.h>
#include <iDynTree/Sensors/PredictSensorsMeasurements.h>

#include <iDynTree/ModelIO/ModelLoader.h>
#include <iDynTree/Estimation/ExternalWrenchesEstimation.h>
#include <iDynTree/yarp/YARPConversions.h>
#include <iDynTree/skinDynLibConversions.h>

using namespace iCub::iDyn;
using namespace iCub::ctrl;
using namespace iCub::iDynTree;
using namespace yarp::sig;
using namespace yarp::math;
using namespace yarp::os;
using namespace iCub::skinDynLib;

using namespace std;

void set_random_vector(yarp::sig::Vector & vec, yarp::os::Random & rng, double coeff=1.0)
{
    for( int i=0; i < (int)vec.size(); i++ ) {
        vec[i] =  coeff*M_PI*rng.uniform();
    }
}

Matrix adjoint_twist(Matrix H)
{
    Matrix R = H.submatrix(0,2,0,2);
    Vector r = H.submatrix(0,2,0,3).getCol(3);
    //std::cout << "~~~~~~~ " << std::endl << " r " << r.toString() << std::endl;
    Matrix adj(6,6);
    adj.zero();
    adj.setSubmatrix(R,0,0);
    adj.setSubmatrix(R,3,3);
    adj.setSubmatrix(crossProductMatrix(r)*R,0,3);
    return adj;
}

Matrix SE3Inv(const Matrix &H)
{
    if ((H.rows()!=4) || (H.cols()!=4))
    {

        return Matrix(0,0);
    }

    Vector p(4);
    p[0]=H(0,3);
    p[1]=H(1,3);
    p[2]=H(2,3);
    p[3]=1.0;

    Matrix invH=H.transposed();
    p=invH*p;

    invH(0,3)=-p[0];
    invH(1,3)=-p[1];
    invH(2,3)=-p[2];
    invH(3,0)=invH(3,1)=invH(3,2)=0.0;

    return invH;
}



void iDyn_print_velocity_acceleration(const yarp::sig::Vector & lin_vel, const yarp::sig::Vector & ang_vel, const yarp::sig::Vector & lin_acc, const yarp::sig::Vector & ang_acc, std::string link_name)
{
    Vector v(6), a(6);
    v.setSubvector(0,lin_vel);
    v.setSubvector(3,ang_vel);

    a.setSubvector(0,lin_acc);
    a.setSubvector(3,ang_acc);

    std::cout <<"Velocity of the " << link_name << endl
    << v.toString() << endl
    <<"Acceleration of the " << link_name << endl
    << a.toString() << endl;
    return;
}

void iDyn_compose_velocity_acceleration(const yarp::sig::Vector & lin_vel, const yarp::sig::Vector & ang_vel, const yarp::sig::Vector & lin_acc, const yarp::sig::Vector & ang_acc, Vector & v, Vector & a)
{
    v = Vector(6);
    a = Vector(6);
    v.setSubvector(0,lin_vel);
    v.setSubvector(3,ang_vel);

    a.setSubvector(0,lin_acc);
    a.setSubvector(3,ang_acc);
    return;
}


void iDynTree_print_velocity_acceleration(DynTree & icub_idyntree, const std::string link_name)
{
    std::cerr <<"Velocity of the " << link_name << endl;
    cerr << icub_idyntree.getVel(icub_idyntree.getLinkIndex((link_name)),true).toString() << endl;
    cerr <<"iDynTree Acceleration of " << link_name << endl;
    cerr << icub_idyntree.getAcc(icub_idyntree.getLinkIndex(link_name),true).toString() << endl;
}

void iDynTree_check_velocity(DynTree & icub_idyntree, const std::string link_name,
                             iDynTree::Model & model, iDynTree::LinkVelArray & new_vels, iDynTree::LinkAccArray & properAccs)
{
    Vector v = icub_idyntree.getVel(icub_idyntree.getLinkIndex((link_name)),true);
    iDynTree::Vector3 omegaYarp;
    iDynTree::toiDynTree(v.subVector(3,5),omegaYarp);

    ASSERT_EQUAL_VECTOR(omegaYarp,new_vels(model.getLinkIndex(link_name)).getAngularVec3());

    Vector a = icub_idyntree.getAcc(icub_idyntree.getLinkIndex((link_name)),true);
    iDynTree::Vector3 dotOmegaYarp;
    iDynTree::toiDynTree(a.subVector(3,5),dotOmegaYarp);

    ASSERT_EQUAL_VECTOR(dotOmegaYarp,properAccs(model.getLinkIndex(link_name)).getAngularVec3());
}

void set_random_IMU_q_dq_ddq(yarp::os::Random & rng, DynTree & icub_tree,
                             iDynTree::Vector3 & base_classicalProperAcc, iDynTree::Vector3 & base_angularVel, iDynTree::Vector3 & base_angularAcc,
                             iDynTree::JointPosDoubleArray& jointPos, iDynTree::JointDOFsDoubleArray& jointVel, iDynTree::JointDOFsDoubleArray& jointAcc,
                             double pos_c = 1.0, double vel_c = 1.0, double acc_c = 1.0
)
{
    Vector q(icub_tree.getNrOfDOFs());
    set_random_vector(q,rng,pos_c);

    q = icub_tree.setAng(q);
    iDynTree::toiDynTree(q,jointPos);

    Vector dq(icub_tree.getNrOfDOFs());
    set_random_vector(dq,rng,vel_c);
    dq = icub_tree.setDAng(dq);
    iDynTree::toiDynTree(dq,jointVel);


    Vector ddq(icub_tree.getNrOfDOFs());
    set_random_vector(ddq,rng,acc_c);
    ddq = icub_tree.setD2Ang(ddq);
    iDynTree::toiDynTree(ddq,jointAcc);


    Vector imu_ang_vel(3), imu_ang_acc(3), imu_lin_acc(3);
    set_random_vector(imu_ang_vel,rng,vel_c);
    set_random_vector(imu_ang_acc,rng,acc_c);
    set_random_vector(imu_lin_acc,rng,acc_c);
    iDynTree::toiDynTree(imu_ang_vel,base_angularVel);
    iDynTree::toiDynTree(imu_ang_acc,base_angularAcc);
    iDynTree::toiDynTree(imu_lin_acc,base_classicalProperAcc);

    icub_tree.setInertialMeasure(imu_ang_vel,imu_ang_acc,imu_lin_acc);

    return;
}

std::vector<iDynTree::FTSensorData> get_default_ft_sensors(std::vector<std::string> ft_serialization)
{
    std::vector<iDynTree::FTSensorData> ret;
    for(size_t i =0; i < ft_serialization.size(); i++ )
    {
        iDynTree::FTSensorData dat;
        dat.reference_joint = ft_serialization[i];
        dat.sensor_name = dat.reference_joint+"_sensor";
        dat.measure_direction = iDynTree::FTSensorData::CHILD_TO_PARENT;
        dat.frame = iDynTree::FTSensorData::CHILD_LINK_FRAME;
        dat.sensor_pose = KDL::Frame::Identity();

        ret.push_back(dat);
    }

    return ret;
}

bool addDoubleContact(iCub::iDynTree::TorqueEstimationTree & estimation_model,
                      const iDynTree::Model & model,
                      iDynTree::skinDynLibConversionsHelper & icub_skin_helper,
                      dynContactList & input_contact_list,
                      const std::string contactLink, const std::string contactFrame)
{
    int contactLinkIndex =  estimation_model.getLinkIndex(contactLink);
    estimation_model.addSkinDynLibAlias(contactLink,contactFrame,0,contactLinkIndex);
    icub_skin_helper.addSkinDynLibAlias(model,contactLink,contactFrame,0,contactLinkIndex);

    yarp::sig::Vector oneContactPoint(3);
    oneContactPoint.zero();
    oneContactPoint[0] = 1.0;
    dynContact contactOne((iCub::skinDynLib::BodyPart) 0, contactLinkIndex,oneContactPoint);
    input_contact_list.push_back(contactOne);

    yarp::sig::Vector twoContactPoint(3);
    twoContactPoint.zero();
    twoContactPoint[1] = 1.0;
    dynContact contactTwo((iCub::skinDynLib::BodyPart) 0, contactLinkIndex,twoContactPoint);
    input_contact_list.push_back(contactTwo);

    return true;
}

bool addSingleContact(iCub::iDynTree::TorqueEstimationTree & estimation_model,
                      const iDynTree::Model & model,
                      iDynTree::skinDynLibConversionsHelper & icub_skin_helper,
                      dynContactList & input_contact_list,
                      const std::string contactLink, const std::string contactFrame)
{
    int contactLinkIndex =  estimation_model.getLinkIndex(contactLink);
    estimation_model.addSkinDynLibAlias(contactLink,contactFrame,0,contactLinkIndex);
    icub_skin_helper.addSkinDynLibAlias(model,contactLink,contactFrame,0,contactLinkIndex);
    yarp::sig::Vector oneContactPoint(3);
    oneContactPoint.zero();
    oneContactPoint[0] = 1.0;
    dynContact contactOne((iCub::skinDynLib::BodyPart) 0, contactLinkIndex,oneContactPoint);
    input_contact_list.push_back(contactOne);

    return true;
}

void getJointSerializationFromDynTree(DynTree & dynTree,
                                      const iDynTree::Model & model,
                                      std::vector<std::string>& joints)
{
    joints.resize(0);
    size_t nrOfJoints = dynTree.getNrOfLinks()-1;
    for(size_t j=0; j < nrOfJoints; j++ )
    {
        std::string jointName;
        bool ok = dynTree.getJunctionName(j,jointName);
        assert(ok);

        // if the joints is also in the model, add it
        if( model.isJointNameUsed(jointName) )
        {
            joints.push_back(jointName);
        }
    }
}

int main(int argc, char ** argv)
{
    //Initializing the random number generator
    yarp::os::Random rng;
    rng.seed(0.0);

    std::vector<std::string> ft_serialization;
    ft_serialization.push_back("l_arm_ft_sensor");
    ft_serialization.push_back("r_arm_ft_sensor");
    ft_serialization.push_back("l_leg_ft_sensor");
    ft_serialization.push_back("l_foot_ft_sensor");
    ft_serialization.push_back("r_leg_ft_sensor");
    ft_serialization.push_back("r_foot_ft_sensor");

    std::vector<std::string> dof_serialization = std::vector<std::string>(0);

    //Creating the DynTree : kinematic/dynamics structure, force torque sensors, imu sensor
    std::string urdf_filename = getAbsModelPath("icub_skin_frames.urdf");
    std::cerr << "Creating TorqueEstimationTree from file " << urdf_filename << std::endl;
    iCub::iDynTree::TorqueEstimationTree * icub_model_estimation =
        new iCub::iDynTree::TorqueEstimationTree(urdf_filename,dof_serialization,ft_serialization,"imu_frame");

    // Creating a model from file just to get the joints
    iDynTree::ModelLoader preliminaryLoader;
    preliminaryLoader.loadModelFromFile(urdf_filename);

    // Get the same serialization of joints used in iDynTree
    std::vector<std::string> usedJoints;
    getJointSerializationFromDynTree(*icub_model_estimation,preliminaryLoader.model(),usedJoints);

    // Get model with exactly the same serialization of the legacy DynTree class
    iDynTree::ModelLoader loader;
    bool ok = loader.loadReducedModelFromFile(urdf_filename, usedJoints);

    if( !ok )
    {
        std::cerr << "Error in creating reduced model" << std::endl;
        return EXIT_FAILURE;
    }

    iDynTree::Model model = loader.model();

    assert(model.getNrOfDOFs() == icub_model_estimation->getNrOfDOFs());

    iDynTree::Traversal traversal, head_traversal;
    // Get the IMU Frame
    iDynTree::FrameIndex imu_index = model.getFrameIndex("imu_frame");
    iDynTree::LinkIndex head_index = model.getFrameLink(imu_index);
    iDynTree::Transform head_T_imu = model.getFrameTransform(imu_index);

    model.computeFullTreeTraversal(traversal);
    ok = model.computeFullTreeTraversal(head_traversal,head_index);

    ASSERT_EQUAL_DOUBLE(ok,true);

    // Create sensors list with FT sensors (TODO)
    iDynTree::SensorsList sensors = loader.sensors();
    sensors.setSerialization(iDynTree::SIX_AXIS_FORCE_TORQUE,ft_serialization);

    // Create submodel decomposition

    iDynTree::SubModelDecomposition subModels;
    ok = subModels.splitModelAlongJoints(model,traversal,ft_serialization);
    if( !ok )
    {
        std::cerr << "Error in splitting the model in submodels" << std::endl;
        return EXIT_FAILURE;
    }

    iDynTree::skinDynLibConversionsHelper iCubSkinHelper;

    // Base quantities
    iDynTree::Vector3 base_classicalProperAcc,base_angularVel,base_angularAcc;

    // position/velocity/acceleration
    iDynTree::JointPosDoubleArray jointPos(model);
    iDynTree::JointDOFsDoubleArray jointVel(model);
    iDynTree::JointDOFsDoubleArray jointAcc(model);

    // Data structures to encode the external forces
    iDynTree::LinkNetExternalWrenches netExternalWrenches(model);
    iDynTree::LinkContactWrenches     externalWrenches(model);

    // Data structure to encode the joint torques
    iDynTree::FreeFloatingGeneralizedTorques trqs(model);

    // Extend the freefloating state classes to easily set
    // the velocity/acceleration/force of the base using the different conventions

    //Assign random kinematic state
    set_random_IMU_q_dq_ddq(rng,*icub_model_estimation,
                            base_classicalProperAcc, base_angularVel,base_angularAcc,
                            jointPos,jointVel,jointAcc);

    //Assign random sensor_readings
    Vector ft_sensor(6);

    iDynTree::SensorsMeasurements sensMeas(sensors);

    ASSERT_EQUAL_DOUBLE(icub_model_estimation->getNrOfFTSensors(),6);

    for(int i = 0; i < icub_model_estimation->getNrOfFTSensors(); i++ )
    {
        set_random_vector(ft_sensor,rng,1.0);
        icub_model_estimation->setSensorMeasurement(i,ft_sensor);
        iDynTree::Wrench ft_sensor_idyntree;
        iDynTree::toiDynTree(ft_sensor,ft_sensor_idyntree);
        assert(sensors.getNrOfSensors(iDynTree::SIX_AXIS_FORCE_TORQUE) > i);
        ASSERT_EQUAL_STRING(ft_serialization[i],sensors.getSensor(iDynTree::SIX_AXIS_FORCE_TORQUE,i)->getName());
        sensMeas.setMeasurement(iDynTree::SIX_AXIS_FORCE_TORQUE,i,ft_sensor_idyntree);
    }

    dynContactList input_contact_list, output_contact_list;

    // Populate input list and mapping between skinDynLib links and
    // iDynTree links
    addSingleContact(*icub_model_estimation,model,iCubSkinHelper,input_contact_list,
                     "chest","chest_skin_frame");
    addSingleContact(*icub_model_estimation,model,iCubSkinHelper,input_contact_list,
                     "l_hand","l_hand_dh_frame");
    addSingleContact(*icub_model_estimation,model,iCubSkinHelper,input_contact_list,
                     "r_hand","r_hand_dh_frame");
    addSingleContact(*icub_model_estimation,model,iCubSkinHelper,input_contact_list,
                     "r_foot","r_foot_dh_frame");
    addSingleContact(*icub_model_estimation,model,iCubSkinHelper,input_contact_list,
                     "l_foot","l_foot_dh_frame");
    addSingleContact(*icub_model_estimation,model,iCubSkinHelper,input_contact_list,
                     "l_lower_leg","l_lower_leg");
    addSingleContact(*icub_model_estimation,model,iCubSkinHelper,input_contact_list,
                     "r_lower_leg","r_lower_leg");

    addDoubleContact(*icub_model_estimation,model,iCubSkinHelper,input_contact_list,
                     "chest","chest_skin_frame");
    addDoubleContact(*icub_model_estimation,model,iCubSkinHelper,input_contact_list,
                     "l_hand","l_hand_dh_frame");
    addDoubleContact(*icub_model_estimation,model,iCubSkinHelper,input_contact_list,
                     "r_hand","r_hand_dh_frame");
    addDoubleContact(*icub_model_estimation,model,iCubSkinHelper,input_contact_list,
                     "r_foot","r_foot_dh_frame");
    addDoubleContact(*icub_model_estimation,model,iCubSkinHelper,input_contact_list,
                     "l_foot","l_foot_dh_frame");
    addDoubleContact(*icub_model_estimation,model,iCubSkinHelper,input_contact_list,
                     "l_lower_leg","l_lower_leg");
    addDoubleContact(*icub_model_estimation,model,iCubSkinHelper,input_contact_list,
                     "r_lower_leg","r_lower_leg");

    // Create iDynTree-compatible input list
    iDynTree::LinkUnknownWrenchContacts unknownContacts(model);

    iCubSkinHelper.fromSkinDynLibToiDynTree(model,input_contact_list,unknownContacts);


    icub_model_estimation->setContacts(input_contact_list);

    icub_model_estimation->kinematicRNEA();
    icub_model_estimation->estimateContactForcesFromSkin();
    ok = icub_model_estimation->dynamicRNEA();

    output_contact_list = icub_model_estimation->getContacts();

    if( output_contact_list.size() != input_contact_list.size() )
    {
        std::cout << "Error: Estimated a wrong number of contacts.";
        return EXIT_FAILURE;
    }

    // Compute the estimated torque with iDynTree
    // Allocate some buffers
    iDynTree::LinkVelArray linkVels(model);
    iDynTree::LinkAccArray linkProperAccs(model);
    iDynTree::estimateExternalWrenchesBuffers extWrenchBufs(subModels);
    iDynTree::LinkInternalWrenches internalWrenches(model);

    // Run the kinematics loop
    // The old yarp interface takes the measurement in imu_frame orientation, while the one in head,
    // so we need to comptue the different acceleration/velocity to input in the system
    /*
    std::cerr << " base classical proper acc " << base_classicalProperAcc.toString() << std::endl;
    std::cerr << "joint pos : " << jointPos.toString() << std::endl;
    std::cerr << "joint vel : " << jointVel.toString() << std::endl;*/
    iDynTree::Vector3 base_angularVel_head;
    iDynTree::Rotation head_R_imu = head_T_imu.getRotation();

    iDynTree::SpatialAcc base_acc_imu;
    iDynTree::SpatialAcc base_acc_head;

    iDynTree::Twist base_vel_imu, base_vel_head;
    iDynTree::Vector3 zero3;
    zero3.zero();
    base_vel_imu.setLinearVec3(zero3);
    base_vel_imu.setAngularVec3(base_angularVel);

    base_vel_head = head_T_imu*base_vel_imu;

    base_acc_imu.setLinearVec3(base_classicalProperAcc);
    base_acc_imu.setAngularVec3(base_angularAcc);

    ASSERT_EQUAL_VECTOR(base_acc_imu.getLinearVec3(),base_classicalProperAcc);
    ASSERT_EQUAL_VECTOR(base_acc_imu.getAngularVec3(),base_angularAcc);


    base_acc_head = head_T_imu*base_acc_imu;

    iDynTree::ClassicalAcc base_acc_head_classical;
    base_acc_head_classical.fromSpatial(base_acc_head,base_vel_head);

    /*
    iDynTree::dynamicsEstimationForwardVelAccKinematics(model,head_traversal,
                                              base_acc_head.getLinearVec3(),base_vel_head.getAngularVec3(),base_acc_head.getAngularVec3(),
                                              jointPos,jointVel,jointAcc,linkVels,linkProperAccs);*/
    iDynTree::dynamicsEstimationForwardVelAccKinematics(model,head_traversal,
                                              base_acc_head_classical.getLinearVec3(),base_vel_head.getAngularVec3(),base_acc_head_classical.getAngularVec3(),
                                              jointPos,jointVel,jointAcc,linkVels,linkProperAccs);

    for(int ii=0; ii < model.getNrOfLinks(); ii++)
    {
        iDynTree_check_velocity(*icub_model_estimation,model.getLinkName(ii),model,linkVels,linkProperAccs);
    }

    // Estimate the external forces
    iDynTree::estimateExternalWrenches(model,subModels,sensors,unknownContacts,
                                       jointPos,linkVels,linkProperAccs,
                                       sensMeas,extWrenchBufs,externalWrenches);

    // Result
    // Convert the external wrenches in a format suitable for RNEA
    externalWrenches.computeNetWrenches(netExternalWrenches);

    // Compute the joint torques
    iDynTree::RNEADynamicPhase(model,traversal,jointPos,linkVels,linkProperAccs,
                               netExternalWrenches,internalWrenches,trqs);

    // Simulate the sensors and make sure that they are consistent
    iDynTree::SensorsMeasurements simulatedSensors(sensors);
    iDynTree::predictSensorsMeasurementsFromRawBuffers(model,sensors,traversal,
                                                       linkVels,linkProperAccs,internalWrenches,
                                                       simulatedSensors);

    for(size_t simFT=0; simFT < simulatedSensors.getNrOfSensors(iDynTree::SIX_AXIS_FORCE_TORQUE); simFT++)
    {
        iDynTree::Wrench simWrench;
        simulatedSensors.getMeasurement(iDynTree::SIX_AXIS_FORCE_TORQUE,simFT,simWrench);
        iDynTree::Wrench actualWrench;
        sensMeas.getMeasurement(iDynTree::SIX_AXIS_FORCE_TORQUE,simFT,actualWrench);

        ASSERT_EQUAL_SPATIAL_FORCE(actualWrench,simWrench);
    }

    // The base "generalized torque" should be zero if the netExternalWrenches
    // were consistent with the accelerations
    iDynTree::SpatialForceVector zero = iDynTree::SpatialForceVector::Zero();
    ASSERT_EQUAL_SPATIAL_FORCE(trqs.baseWrench(),zero);

    // Compute the
    dynContactList output_contact_list_with_idyntree;
    iCubSkinHelper.fromiDynTreeToSkinDynLib(model,externalWrenches,output_contact_list_with_idyntree);

    if( output_contact_list_with_idyntree.size() != input_contact_list.size() )
    {
        std::cerr << "Error: Estimated a wrong number of contacts in iDynTree.";
        return EXIT_FAILURE;
    }

    // Test also another (equivalent) method
    skinContactList outputSkinContactListWithIdynTree;
    iCubSkinHelper.updateSkinContactListFromLinkContactWrenches(model,externalWrenches,outputSkinContactListWithIdynTree);

    if( outputSkinContactListWithIdynTree.size() != input_contact_list.size() )
    {
        std::cerr << "Error: Estimated a wrong number of contacts in iDynTree.";
        return EXIT_FAILURE;
    }

    if( outputSkinContactListWithIdynTree.size() == 0 )
    {
        std::cerr << "Error: no forces have been estimated.";
        return EXIT_FAILURE;
    }

    // Compare the results
    /*
    std::cerr << "Joint torques with KDL/YARP : " << std::endl;
    std::cerr << icub_model_estimation->getTorques().toString() << std::endl;
    std::cerr << "Joint torques with iDynTree: " << std::endl;
    std::cerr << toEigen(trqs.jointTorques()) << std::endl;
    */

    iDynTree::VectorDynSize jointTorquesYARP(icub_model_estimation->getTorques().size());

    iDynTree::toiDynTree(icub_model_estimation->getTorques(),jointTorquesYARP);

    ASSERT_EQUAL_VECTOR_TOL(trqs.jointTorques(),jointTorquesYARP,1e-4);

    delete icub_model_estimation;

    if( !ok )
    {
        std::cerr << "Error in estimation of joint torques" << std::endl;
        return EXIT_FAILURE;
    }

    return EXIT_SUCCESS;
}
