<commit_before>/**
* @ingroup idyntree_tutorials
*
*
* A tutorial on how to the gravity compensation terms for a fixed
*  base manipulator.
*
* \author <NAME>
*
* CopyPolicy: Released under the terms of GPL 2.0 or later
*/

// YARP headers
#include <yarp/os/ResourceFinder.h>
#include <yarp/os/LogStream.h>

// iDynTree headers
#include <iCub/iDynTree/DynTree.h>

#include <iDynTree/Core/Twist.h>
#include <iDynTree/Core/MatrixDynSize.h>
#include <iDynTree/Core/VectorDynSize.h>
#include <iDynTree/Regressors/DynamicsRegressorGenerator.h>

#include <urdf_exception/exception.h>

#include <string>
#include <iostream>

/*****************************************************************/
int main(int argc, char *argv[])
{
    // In this example we use yarp to parse the configuration
    // parameters
    yarp::os::ResourceFinder rf;
    rf.setVerbose(true);
    rf.setDefault("name","fixedBaseGravityCompensation");
    rf.setDefault("config","config.ini");
    rf.configure(argc,argv);

    if (rf.check("help"))
    {
        yInfo() << "Options:\n\n";
        yInfo() << "\t--urdf    file: name of the URDF file containing"
                   " the model of the robot.  \n";

        return 0;
    }

    std::string urdf_filename = rf.findFile("../bigman_left_arm.urdf");
    yInfo() << "Trying to open " << urdf_filename << " as robot model";

    iDynTree::Regressors::DynamicsRegressorGenerator generator;
    generator.loadRobotAndSensorsModelFromFile(urdf_filename);

    const char* regrXml =
        "<regressor>"
        "    <jointTorqueDynamics>"
        "      <joints>"
        "        <joint>LShSag</joint>"
        "        <joint>LShLat</joint>"
        "        <joint>LShYaw</joint>"
        "        <joint>LElbj</joint>"
        "        <joint>LForearmPlate</joint>"
        "        <joint>LWrj1</joint>"
        "        <joint>LWrj2</joint>"
        "      </joints>"
        "    </jointTorqueDynamics>"
        "</regressor>";

    generator.loadRegressorStructureFromString(regrXml);

    std::cout << generator.getDescriptionOfParameters() << std::endl;

    uint N_DOFS = generator.getNrOfDegreesOfFreedom();
    std::cout << "Nr of DOFs:" << N_DOFS << std::endl;
    uint N_OUT = generator.getNrOfOutputs();
    std::cout << "Nr of outputs:" << N_OUT << std::endl;
    uint N_LINKS = generator.getNrOfLinks();
    std::cout << "Nr of links:" << N_LINKS << std::endl;
    std::cout << "Nr of fake links:" << generator.getNrOfFakeLinks() << std::endl;
    uint N_PARAMS = generator.getNrOfParameters();
    std::cout << "Nr of params:" << N_PARAMS << std::endl;
    iDynTree::VectorDynSize cadParams(N_PARAMS);
    generator.getModelParameters(cadParams);

    iDynTree::Twist gravity_twist;
    gravity_twist.zero();
    gravity_twist.setVal(2, -9.81);

    iDynTree::VectorDynSize qq (N_DOFS);
    iDynTree::VectorDynSize dq (N_DOFS);
    iDynTree::VectorDynSize ddq (N_DOFS);

    /*for (uint dof=0; dof<N_DOFS; dof++) {
        qq.setVal(dof, 1.0);
        dq.setVal(dof, 0.01);
        ddq.setVal(dof, 0.001);
    }*/

    generator.setRobotState(qq,dq,ddq, gravity_twist);
    generator.setTorqueSensorMeasurement(0, 0.05);

    iDynTree::MatrixDynSize regressor(N_OUT, N_PARAMS);  //Y
    iDynTree::VectorDynSize knownTerms(N_OUT);  //corresponds to measured torques in this case
    if (!generator.computeRegressor(regressor, knownTerms)) {
        std::cout << "Error while computing regressor" << std::endl;
    }

    std::cout << regressor.toString() << std::endl;


    return EXIT_SUCCESS;
}
<commit_msg>undo accidental commit of testing code<commit_after>/**
* @ingroup idyntree_tutorials
*
*
* A tutorial on how to the gravity compensation terms for a fixed
*  base manipulator.
*
* \author <NAME>
*
* CopyPolicy: Released under the terms of GPL 2.0 or later
*/

// YARP headers
#include <yarp/os/ResourceFinder.h>
#include <yarp/os/LogStream.h>

// iDynTree headers
#include <iCub/iDynTree/DynTree.h>
#include <urdf_exception/exception.h>

#include <string>
#include <iostream>

/*****************************************************************/
int main(int argc, char *argv[])
{
    // In this example we use yarp to parse the configuration
    // parameters
    yarp::os::ResourceFinder rf;
    rf.setVerbose(true);
    rf.setDefault("name","fixedBaseGravityCompensation");
    rf.setDefault("config","config.ini");
    rf.configure(argc,argv);

    if (rf.check("help"))
    {
        yInfo() << "Options:\n\n";
        yInfo() << "\t--urdf    file: name of the URDF file containing"
                   " the model of the robot.  \n";

        return 0;
    }

    std::string urdf_filename = rf.findFile("urdf");


    yInfo() << "Tryng to open " << urdf_filename << " as robot model";
    iCub::iDynTree::DynTree robot_model;
    bool ok = robot_model.loadURDFModel(urdf_filename);

    if( !ok )
    {
        std::cerr << "Loading urdf file " << urdf_filename << " failed, exiting" << std::endl;
        return EXIT_FAILURE;
    }

    //
    // Now we will set the variable necessary to compute the gravity compensation terms
    //

    // The gravity acceleration vector should be expressed in the base link frame,
    // in this example we assume that this frame has the Z axis pointing up
    // All the inputs are expressed in SI unit of measure, so the gravity acceleration
    // is expressed in m/s^2
    yarp::sig::Vector grav(3);
    grav(0) = grav(1) = 0.0;
    grav(2) = -9.8;

    robot_model.setGravity(grav);

    // We now need to set the joint position for the desired gravity compensation
    // For the sake of example, we will put all the joint positions to 10.0 degrees;
    yarp::sig::Vector q(robot_model.getNrOfDOFs());

    double deg2rad = 3.14/180.0;

    for(int i =0; i < q.size(); i++ )
    {

        q(i) = deg2rad*10.0;
    }

    // Now we set the joint values
    robot_model.setAng(q);

    // Now we can call the RNEA algorithm to compute the gravity torques
    robot_model.kinematicRNEA();
    robot_model.dynamicRNEA();

    // The obtained torques are expressed in Nm
    yarp::sig::Vector gravity_torques = robot_model.getTorques();

    // Output some result
    std::cout << "Input gravity : " << grav.toString() << std::endl;
    std::cout << "Input joint positions : " << robot_model.getAng().toString() << std::endl;
    std::cout << "Output joint torques  : " << gravity_torques.toString() << std::endl;


    return EXIT_SUCCESS;
}

{'fraction_non_alphanumeric': 0.15680794907623039, 'fraction_numerical': 0.007918025151374011, 'mean_word_length': 3.0927573062261753, 'pattern_counts': {'":': 0, '<': 131, '<?xml version=': 0, '>': 41, 'https://': 0, 'lorem ipsum': 0, 'www.': 0, 'xml': 0}, 'pii_count': 0, 'substrings_counts': 0, 'word_list_counts': {'cursed_substrings.json': 0, 'profanity_word_list.json': 0, 'sexual_word_list.json': 0, 'zh_pornsignals.json': 0}}
{'dir': 'git-commits-cleaned', 'id': '5620349', 'n_tokens_mistral': 2234, 'n_tokens_neox': 2138, 'n_words': 512}