// SPDX-FileCopyrightText: Fondazione Istituto Italiano di Tecnologia (IIT)
// SPDX-License-Identifier: BSD-3-Clause

#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/ContactWrench.h>
#include <iDynTree/Dynamics.h>
#include <iDynTree/ForwardKinematics.h>
#include <iDynTree/FreeFloatingState.h>
#include <iDynTree/JointState.h>
#include <iDynTree/LinkState.h>
#include <iDynTree/Model.h>
#include <iDynTree/ModelTransformers.h>
#include <iDynTree/SubModel.h>
#include <iDynTree/Traversal.h>

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

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

#include <iDynTree/ExternalWrenchesEstimation.h>
#include <iDynTree/ModelLoader.h>
#include <iDynTree/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;
}
