<commit_before>// -*- mode:C++ { } tab-width:4 { } c-basic-offset:4 { } indent-tabs-mode:nil -*-

/*
 * Copyright (C) 2015 iCub Facility
 * Authors: <NAME>
 * CopyPolicy: Released under the terms of the LGPLv2.1 or later, see LGPL.TXT
 *
 */

#include <math.h>
#include <rtf/TestAssert.h>
#include <yarp/os/Time.h>
#include <rtf/yarp/JointsPosMotion.h>

using namespace RTF;
using namespace RTF::YARP;
using namespace yarp::os;
using namespace yarp::dev;



//jointsPosMotion::jointsPosMotion(){}
void jointsPosMotion::init(yarp::dev::PolyDriver *polydriver)
{

    jointsList = 0;
    n_joints = 0;
    encoders = 0;
    speed = 0;

    tolerance = 1.0;
    timeout = 5.0;

    dd = polydriver;
    RTF_ASSERT_ERROR_IF(dd->isValid(),"Unable to open device driver");
    RTF_ASSERT_ERROR_IF(dd->view(ienc),"Unable to open encoders interface");
    RTF_ASSERT_ERROR_IF(dd->view(ipos),"Unable to open position interface");
    RTF_ASSERT_ERROR_IF(dd->view(icmd),"Unable to open control mode interface");
    RTF_ASSERT_ERROR_IF(dd->view(iimd),"Unable to open interaction mode interface");
    RTF_ASSERT_ERROR_IF(dd->view(ilim),"Unable to open limits interface");

}


jointsPosMotion::jointsPosMotion(yarp::dev::PolyDriver *polydriver, yarp::sig::Vector &jlist)
{
    init(polydriver);

    n_joints = jlist.size();
    jointsList.resize(n_joints);
    jointsList = jlist;

    encoders.resize(n_joints); encoders.zero();
    speed = yarp::sig::Vector(n_joints, 10.0);

    //send default speed
    for (unsigned int i=0; i<n_joints; i++)
    {
        ipos->setRefSpeed((int)jointsList[i],speed[i]);
    }
    readJointsLimits();

}

jointsPosMotion::~jointsPosMotion()
{
   // if (dd) {delete dd; dd =0;}
}


void jointsPosMotion::setTolerance(double tolerance) {tolerance = tolerance;}


bool jointsPosMotion::setAndCheckPosControlMode()
{
    for (unsigned int i=0; i<jointsList.size(); i++)
    {
        icmd->setControlMode((int)jointsList[i],VOCAB_CM_POSITION);
        iimd->setInteractionMode((int)jointsList[i],VOCAB_IM_STIFF);
        yarp::os::Time::delay(0.010);
    }

    int cmode;
    yarp::dev::InteractionModeEnum imode;
    double time_started = yarp::os::Time::now();

    while (1)
    {
        int ok=0;
        for (unsigned int i=0; i<n_joints; i++)
        {
            icmd->getControlMode ((int)jointsList[i],&cmode);
            iimd->getInteractionMode((int)jointsList[i],&imode);
            if (cmode==VOCAB_CM_POSITION && imode==VOCAB_IM_STIFF) ok++;
        }
        if (ok==n_joints) break;
        if (yarp::os::Time::now()-time_started > timeout)
        {
            RTF_ASSERT_ERROR("Unable to set control mode/interaction mode");
        }

        yarp::os::Time::delay(0.2);

    }

}

void jointsPosMotion::setTimeout(double timeout){timeout = timeout;}

int jointsPosMotion::getIndexOfJoint(int j)
{
    for(int i=0; i<n_joints; i++)
    {
        if(jointsList[i] == j)
            return i;
    }
    return jointsList.size()+1;
}<commit_msg>fixed missing return value<commit_after>// -*- mode:C++ { } tab-width:4 { } c-basic-offset:4 { } indent-tabs-mode:nil -*-

/*
 * Copyright (C) 2015 iCub Facility
 * Authors: <NAME>
 * CopyPolicy: Released under the terms of the LGPLv2.1 or later, see LGPL.TXT
 *
 */

#include <math.h>
#include <rtf/TestAssert.h>
#include <yarp/os/Time.h>
#include <rtf/yarp/JointsPosMotion.h>

using namespace RTF;
using namespace RTF::YARP;
using namespace yarp::os;
using namespace yarp::dev;



//jointsPosMotion::jointsPosMotion(){}
void jointsPosMotion::init(yarp::dev::PolyDriver *polydriver)
{

    jointsList = 0;
    n_joints = 0;
    encoders = 0;
    speed = 0;

    tolerance = 1.0;
    timeout = 5.0;

    dd = polydriver;
    RTF_ASSERT_ERROR_IF(dd->isValid(),"Unable to open device driver");
    RTF_ASSERT_ERROR_IF(dd->view(ienc),"Unable to open encoders interface");
    RTF_ASSERT_ERROR_IF(dd->view(ipos),"Unable to open position interface");
    RTF_ASSERT_ERROR_IF(dd->view(icmd),"Unable to open control mode interface");
    RTF_ASSERT_ERROR_IF(dd->view(iimd),"Unable to open interaction mode interface");
    RTF_ASSERT_ERROR_IF(dd->view(ilim),"Unable to open limits interface");

}


jointsPosMotion::jointsPosMotion(yarp::dev::PolyDriver *polydriver, yarp::sig::Vector &jlist)
{
    init(polydriver);

    n_joints = jlist.size();
    jointsList.resize(n_joints);
    jointsList = jlist;

    encoders.resize(n_joints); encoders.zero();
    speed = yarp::sig::Vector(n_joints, 10.0);

    //send default speed
    for (unsigned int i=0; i<n_joints; i++)
    {
        ipos->setRefSpeed((int)jointsList[i],speed[i]);
    }
    readJointsLimits();

}

jointsPosMotion::~jointsPosMotion()
{
   // if (dd) {delete dd; dd =0;}
}


void jointsPosMotion::setTolerance(double tolerance) {tolerance = tolerance;}


bool jointsPosMotion::setAndCheckPosControlMode()
{
    for (unsigned int i=0; i<jointsList.size(); i++)
    {
        icmd->setControlMode((int)jointsList[i],VOCAB_CM_POSITION);
        iimd->setInteractionMode((int)jointsList[i],VOCAB_IM_STIFF);
        yarp::os::Time::delay(0.010);
    }

    int cmode;
    yarp::dev::InteractionModeEnum imode;
    double time_started = yarp::os::Time::now();

    while (1)
    {
        int ok=0;
        for (unsigned int i=0; i<n_joints; i++)
        {
            icmd->getControlMode ((int)jointsList[i],&cmode);
            iimd->getInteractionMode((int)jointsList[i],&imode);
            if (cmode==VOCAB_CM_POSITION && imode==VOCAB_IM_STIFF) ok++;
        }
        if (ok==n_joints) break;
        if (yarp::os::Time::now()-time_started > timeout)
        {
            RTF_ASSERT_ERROR("Unable to set control mode/interaction mode");
        }

        yarp::os::Time::delay(0.2);

    }

    return true;
}

void jointsPosMotion::setTimeout(double timeout){timeout = timeout;}

int jointsPosMotion::getIndexOfJoint(int j)
{
    for(int i=0; i<n_joints; i++)
    {
        if(jointsList[i] == j)
            return i;
    }
    return jointsList.size()+1;
}
{'fraction_non_alphanumeric': 0.15181243764549385, 'fraction_numerical': 0.010974393082806785, 'mean_word_length': 3.529367469879518, 'pattern_counts': {'":': 0, '<': 21, '<?xml version=': 0, '>': 37, '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': '4732379', 'n_tokens_mistral': 2295, 'n_tokens_neox': 2174, 'n_words': 399}