コード例 #1
0
bool learnPrimitive::updatePrimitive(ResourceFinder &rf){
    Bottle bOutput;

    //1. Primitive
    Bottle bPrimitiveAction = rf.findGroup("Primitive_Action");

    if (!bPrimitiveAction.isNull())
    {
        Bottle * bPrimitiveActionName = bPrimitiveAction.find("primitiveActionName").asList();
        Bottle * bPrimitiveActionArg  = bPrimitiveAction.find("primitiveActionArg").asList();

       if(bPrimitiveActionName->isNull() || bPrimitiveActionArg->isNull()){
           yError() << " [updatePrimitiveAction] : one of the primitiveAction conf is null : primitiveActionName, primitiveActionArg" ;
            return false ;
        }

        int primitiveActionSize = -1 ;
        if(bPrimitiveActionName->size() == bPrimitiveActionArg->size()){
            primitiveActionSize =  bPrimitiveActionName->size() ;
        } else {
            yError() << " [updatePrimitiveAction] : one of the primitiveAction conf has different size!" ;
            return false ;
        }

        if(primitiveActionSize == 0) {
            yWarning() << " [updatePrimitiveAction] : there is no primitiveAction defined at startup!" ;
        } else {

            for(int i = 0; i < primitiveActionSize ; i++) {
                //insert protoaction even if already there
                Bottle bPrimitive;
                string currentPrimName = bPrimitiveActionName->get(i).asString();
                string currentPrimArg  = bPrimitiveActionArg->get(i).asString();

                bPrimitive.addString(currentPrimName);
                bPrimitive.addString(currentPrimArg);

                string concat = currentPrimName + "_" + currentPrimArg;
                Bottle bCurrentPrim = rf.findGroup(concat);
                if(bCurrentPrim.isNull()){
                    yError() << " [updatePrimitiveAction] : " << concat << "is NOT defined" ;
                    return false ;
                }
                Bottle * bListProtoAction = bCurrentPrim.find("actionList").asList();
                if(bListProtoAction->isNull()){
                    yError() << " [updatePrimitiveAction] : " << concat << "is there but is not defined (actionList)" ;
                    return false ;
                }
                bPrimitive.addList() = *bListProtoAction ;
                yInfo() << "Primitive Action added : (" <<bPrimitive.get(0).asString() << ", " << bPrimitive.get(1).asString() << ", ( " << bPrimitive.get(2).asList()->toString()<< "))" ;
                vPrimitiveActionBottle.push_back(bPrimitive);
            }
        }
    } else {
        yError() << " error in learnPrimitive::updatePrimitive | Primitive_Action is NOT defined in the learnPrimitive.ini";
        return false;
    }


    return true;
}
コード例 #2
0
ファイル: main.cpp プロジェクト: pattacini/icub-contrib
    bool configure(ResourceFinder &rf)
    {
        string robot=rf.check("robot",Value("icub")).asString();
        string arm=rf.check("arm",Value("left")).asString();
        string eye=rf.check("eye",Value("left")).asString();
        bool analog=(rf.check("analog",Value(robot=="icub"?"on":"off")).asString()=="on");

        if ((arm!="left") && (arm!="right"))
        {
            yError()<<"Invalid arm requested";
            return false;
        }

        if ((eye!="left") && (eye!="right"))
        {
            yError()<<"Invalid eye requested";
            return false;
        }

        // open drivers
        if (analog)
        {
            Property optionAnalog("(device analogsensorclient)");
            optionAnalog.put("remote","/"+robot+"/"+arm+"_hand/analog:o");
            optionAnalog.put("local","/show-fingers/analog");
            if (!drvAnalog.open(optionAnalog))
            {
                yError()<<"Analog sensor not available";
                terminate();
                return false;
            }
        }

        Property optionArm("(device remote_controlboard)");
        optionArm.put("remote","/"+robot+"/"+arm+"_arm");
        optionArm.put("local","/show-fingers/joints");
        if (!drvArm.open(optionArm))
        {
            yError()<<"Joints arm controller not available";
            terminate();
            return false;
        }

        Property optionCart("(device cartesiancontrollerclient)");
        optionCart.put("remote","/"+robot+"/cartesianController/"+arm+"_arm");
        optionCart.put("local","/show-fingers/cartesian");
        if (!drvCart.open(optionCart))
        {
            yError()<<"Cartesian arm controller not available";
            terminate();
            return false;
        }

        Property optionGaze("(device gazecontrollerclient)");
        optionGaze.put("remote","/iKinGazeCtrl");
        optionGaze.put("local","/show-fingers/gaze");
        if (!drvGaze.open(optionGaze))
        {
            yError()<<"Gaze controller not available";
            terminate();
            return false;
        }

        if (analog)
            drvAnalog.view(ianalog);
        else
            ianalog=NULL;
        IControlLimits *ilim;
        drvArm.view(iencs);
        drvArm.view(ilim);
        drvCart.view(iarm);
        drvGaze.view(igaze);
        iencs->getAxes(&nEncs);

        finger[0]=iCubFinger(arm+"_thumb");
        finger[1]=iCubFinger(arm+"_index");
        finger[2]=iCubFinger(arm+"_middle");

        deque<IControlLimits*> lim;
        lim.push_back(ilim);
        for (int i=0; i<3; i++)
            finger[i].alignJointsBounds(lim);

        yInfo()<<"Using arm="<<arm<<" and eye="<<eye;

        imgInPort.open("/show-fingers/img:i");
        imgOutPort.open("/show-fingers/img:o");

        camSel=(eye=="left")?0:1;
        return true;
    }
コード例 #3
0
ファイル: main.cpp プロジェクト: xufango/contrib_bk
    void setInputPort(ResourceFinder *rf, int index, ConstString local)
    {
        ConstString **remote;
        ConstString *localTmp;
        bool connectionDone = false;
        remote = new ConstString*[nPlots];

        //local/remote port connection
        if(rf->find("remote").isString())
            {
                nPortInputsInPlots[index]  = 1;
                remote[index] = new ConstString[nPortInputsInPlots[index]];
                remote[index][0] = resFind->find("remote").toString();
                plot[index]->setInputPortName(remote[index][0].c_str());
                //connect
                plot[index]->setPorts(&local, nPortInputsInPlots[index]);
                yarpConnectRemoteLocal(remote[index][0], local);
                connectionDone = true;
            }
        else if(rf->find("remote").isList())
            {
                Bottle *rrBot;
                //if (VERBOSE) fprintf(stderr, "MESSAGE: Option remote is a list\n");
                rrBot = resFind->find("remote").asList();
                int listSize = rrBot->size();
                if (rrBot->get(index).isString() && listSize==1)
                    {
                        nPortInputsInPlots[index]  = 1;
                        remote[index] = new ConstString[nPortInputsInPlots[index]];
                        remote[index][0] = rrBot->get(0).toString();
                        plot[index]->setInputPortName(remote[index][0].c_str());
                        //connect
                        plot[index]->setPorts(&local, nPortInputsInPlots[index]);
                        yarpConnectRemoteLocal(remote[index][0], local);
                        connectionDone = true;
                    }
                if (rrBot->get(index).isString() && listSize==nPlots)
                    {
                        nPortInputsInPlots[index]  = 1;
                        remote[index] = new ConstString[nPortInputsInPlots[index]];
                        remote[index][0] = rrBot->get(index).toString();
                        plot[index]->setInputPortName(remote[index][0].c_str());
                        //connect
                        plot[index]->setPorts(&local, nPortInputsInPlots[index]);
                        yarpConnectRemoteLocal(remote[index][0], local);
                        connectionDone = true;
                    }
                if (rrBot->get(index).isList() && listSize==nPlots)
                    {
                        //if (VERBOSE) fprintf(stderr, "MESSAGE: Getting a double list of remote ports! \n");
                        Bottle *rrrBot;
                        rrrBot = rrBot->get(index).asList();
                        nPortInputsInPlots[index]  = rrrBot->size();
                        remote[index] = new ConstString[nPortInputsInPlots[index]];
                        localTmp = new ConstString[nPortInputsInPlots[index]];
                        for (int j = 0; j < nPortInputsInPlots[index]; j++)
                            {
                                char stringN[64];
                                sprintf(stringN, "/input%d", j);
                                ConstString sN(stringN);

                                remote[index][j] = rrrBot->get(j).toString();
                                localTmp[j] = sN;
                                localTmp[j] = localTmp[j] + local;
                            }
                        ConstString sumRemote = remote[index][0];
                        sumRemote = sumRemote + " ";
                        for (int j = 1; j < nPortInputsInPlots[index]; j++)
                            {
                                sumRemote = sumRemote + remote[index][j];
                                sumRemote = sumRemote + " ";
                            }
                        plot[index]->setInputPortName(sumRemote.c_str());
                        //connect
                        plot[index]->setPorts(localTmp, nPortInputsInPlots[index]);
                        for (int j = 0; j < nPortInputsInPlots[index]; j++)
                            yarpConnectRemoteLocal(remote[index][j], localTmp[j]);
                        connectionDone = true;
                    }
                
            }
        if (!connectionDone)
            fprintf(stderr, "WARNING: no input port connected. Waiting explicit connection from user.\n");
    }
コード例 #4
0
ファイル: main.cpp プロジェクト: kt10aan/icub-main
    virtual bool configure(ResourceFinder &rf)
    {
        Time::turboBoost();

        if (rf.check("name"))
            name=string("/")+rf.find("name").asString().c_str();
        else
            name="/trajectoryPlayer";

        rpcPort.open((name+"/rpc").c_str());
        attach(rpcPort);

        Property portProp;
        portProp.put("robot", rf.find("robot"));
        portProp.put("part", rf.find("part"));

        //*** start the robot driver
        if (!robot.configure(portProp))
        {
            yError() << "Error configuring position controller, check parameters";
            return false;
        }
        
        if (!robot.init())
        {
            yError() << "Error cannot connect to remote ports" ;
            return false;
        }

        //*** attach the robot driver to the thread and start it
        w_thread.attachRobotDriver(&robot);
        b_thread.attachRobotDriver(&robot);
        if (!w_thread.start())
        {
            yError() << "Working thread did not start, queue will not work";
        }
        else
        {
            yInfo() << "Working thread started";
        }

        if (rf.check("execute")==true)
        {
            yInfo() << "Enablig iPid->setReference() controller";
            w_thread.enable_execute_joint_command = true;
        }
        else
        {
            yInfo() << "Not using iPid->setReference() controller";
            w_thread.enable_execute_joint_command = false;
        }

        if (rf.check("period")==true)
        {
            int period = rf.find("period").asInt();
            yInfo() << "Thread period set to " << period << "ms";
            w_thread.setRate(period);
        }
        yInfo() << "Using parameters:"  << rf.toString();

        //*** open the position file
        yInfo() << "opening file...";
        if (rf.check("filename")==true)
        {
            string filename = rf.find("filename").asString().c_str();
            int req_joints = 0; //default value
            if (rf.check("joints")) 
            {
                req_joints = rf.find("joints").asInt();}
            else
            {
                yError() << "Missing parameter 'joints' (number of joints to control)";
                return false;
            }

            if (req_joints < robot.n_joints) 
            {
                yWarning () << "changing n joints from" << robot.n_joints << "to" << req_joints;
                robot.n_joints = req_joints;
            }
            else if (req_joints > robot.n_joints)
            {
                yError() << "Requested number of joints exceeds the number of available joints on the robot!";
                return false;
            }

            if (!w_thread.actions.openFile(filename,robot.n_joints))
            {
                yError() << "Unable to parse file " << filename;
                return false;
            };
        }
        else
        {
            yWarning() << "no sequence files load.";
        }

        yInfo() << "module succesffully configured. ready.";
        return true;
    }
コード例 #5
0
bool PlaybackLocomotion::configure(ResourceFinder &rf) {

    int ptModeMs = rf.check("ptModeMs",yarp::os::Value(DEFAULT_PT_MODE_MS),"PT mode miliseconds").asInt();
    CD_INFO("Using ptModeMs: %d (default: %d).\n",ptModeMs,int(DEFAULT_PT_MODE_MS));

    playbackThread.hold = rf.check("hold");

    //-- Open file for reading.
    std::string fileName = rf.check("file",Value(DEFAULT_FILE_NAME),"file name").asString();
    CD_INFO("Using file: %s (default: " DEFAULT_FILE_NAME ").\n",fileName.c_str());
    playbackThread.ifs.open( fileName.c_str() );
    if( ! playbackThread.ifs.is_open() ) {
        CD_ERROR("Could not open file: %s.\n",fileName.c_str());
        return false;
    }
    CD_SUCCESS("Opened file: %s.\n",fileName.c_str());

    //-- left leg --
    std::string leftLegIni = rf.findFileByName("../locomotion/leftLeg.ini");

    yarp::os::Property leftLegOptions;
    if (! leftLegOptions.fromConfigFile(leftLegIni) ) {  //-- Put first because defaults to wiping out.
        CD_ERROR("Could not configure from \"leftLeg.ini\".\n");
        return false;
    }
    CD_SUCCESS("Configured left leg from %s.\n",leftLegIni.c_str());
    leftLegOptions.put("name","/teo/leftLeg");
    leftLegOptions.put("device","CanBusControlboard");
    leftLegOptions.put("ptModeMs",ptModeMs);
    if (rf.check("home")) leftLegOptions.put("home",1);
    if (rf.check("reset")) leftLegOptions.put("reset",1);

    leftLegDevice.open(leftLegOptions);

    if (!leftLegDevice.isValid()) {
        CD_ERROR("leftLegDevice instantiation not worked.\n");
        CD_ERROR("Be sure CMake \"ENABLE_LocomotionYarp_CanBusControlboard\" variable is set \"ON\"\n");
        CD_ERROR("\"SKIP_CanBusControlboard is set\" --> should be --> \"ENABLE_CanBusControlboard is set\"\n");
        // robotDevice.close();  // un-needed?
        return false;
    }

    //-- right leg --
    std::string rightLegIni = rf.findFileByName("../locomotion/rightLeg.ini");

    yarp::os::Property rightLegOptions;
    if (! rightLegOptions.fromConfigFile(rightLegIni) ) {  //-- Put first because defaults to wiping out.
        CD_ERROR("Could not configure from \"rightLeg.ini\".\n");
        return false;
    }
    CD_SUCCESS("Configured right leg from %s.\n",rightLegIni.c_str());
    rightLegOptions.put("name","/teo/rightLeg");
    rightLegOptions.put("device","CanBusControlboard");
    rightLegOptions.put("ptModeMs",ptModeMs);
    if (rf.check("home")) rightLegOptions.put("home",1);
    if (rf.check("reset")) rightLegOptions.put("reset",1);

    rightLegDevice.open(rightLegOptions);

    if (!rightLegDevice.isValid()) {
        CD_ERROR("rightLegDevice instantiation not worked.\n");
        CD_ERROR("Be sure CMake \"ENABLE_LocomotionYarp_CanBusControlboard\" variable is set \"ON\"\n");
        CD_ERROR("\"SKIP_CanBusControlboard is set\" --> should be --> \"ENABLE_CanBusControlboard is set\"\n");
        // robotDevice.close();  // un-needed?
        return false;
    }

    //-- Configure the thread.

    //-- Obtain manipulator interfaces.
    if ( ! leftLegDevice.view( playbackThread.leftLegPos ) ) {
        CD_ERROR("Could not obtain leftLegPos.\n");
        return false;
    }
    CD_SUCCESS("Obtained leftLegPos.\n");

    if ( ! leftLegDevice.view( playbackThread.leftLegPosDirect ) ) {
        CD_ERROR("Could not obtain leftLegPosDirect.\n");
        return false;
    }
    CD_SUCCESS("Obtained leftLegPosDirect.\n");

    if ( ! rightLegDevice.view( playbackThread.rightLegPos ) ) {
        CD_ERROR("Could not obtain leftLegPos.\n");
        return false;
    }
    CD_SUCCESS("Obtained rightLegPos.\n");

    if ( ! rightLegDevice.view( playbackThread.rightLegPosDirect ) ) {
        CD_ERROR("Could not obtain rightLegPosDirect.\n");
        return false;
    }
    CD_SUCCESS("Obtained rightLegPosDirect.\n");

    //-- Do stuff.
    playbackThread.leftLegPos->getAxes( &(playbackThread.leftLegNumMotors) );
    playbackThread.rightLegPos->getAxes( &(playbackThread.rightLegNumMotors) );

    CD_INFO("setPositionDirectMode...\n");
    playbackThread.leftLegPosDirect->setPositionDirectMode();
    playbackThread.rightLegPosDirect->setPositionDirectMode();

    //-- Start the thread.
    CD_INFO("Start thread...\n");
    playbackThread.start();

    return true;
}
コード例 #6
0
int main(int argc, char *argv[])
{
    ResourceFinder rf;
    rf.configure(argc,argv);

    set<string> avFrames=HeadParameters::getTypes();

    string types_helper("");
    for (set<string>::iterator it=avFrames.begin(); it!=avFrames.end(); it++)
        types_helper+=*it+"|";
    types_helper.erase(types_helper.end()-1);

    if (rf.check("help"))
    {
        cout<<"Options:"<<endl;
        cout<<"--type "<<types_helper<<endl;
        cout<<"--verbosity <int>"<<endl;
        cout<<"--xd \"(0.0 1.0 2.0)\""<<endl;
        cout<<"--q0 \"(0.0 1.0 ... 5.0)\""<<endl;
        return 0;
    }

    string type=rf.check("type",Value("gaze")).asString();
    int verbosity=rf.check("verbosity",Value(0)).asInt();

    if (avFrames.find(type)==avFrames.end())
    {
        cerr<<"unrecognized type \""<<type<<"\""<<endl;
        return 1;
    }

    Vector xd(3,0.0);
    if (Bottle *b=rf.find("xd").asList())
    {
        size_t len=std::min(xd.length(),(size_t)b->size());
        for (size_t i=0; i<len; i++)
            xd[i]=b->get(i).asDouble();
    }

    Vector q0(6,0.0);
    if (Bottle *b=rf.find("q0").asList())
    {
        size_t len=std::min(q0.length(),(size_t)b->size());
        for (size_t i=0; i<len; i++)
            q0[i]=b->get(i).asDouble();
    }

    HeadParameters headp(type);
    HeadSolver solver(headp);
    solver.setVerbosity(verbosity);
    solver.setInitialGuess(q0);

    Vector q;
    solver.ikin(xd,q);
    cout<<"head="<<solver.getHeadParameters().head.getType()<<endl; 
    cout<<"q0=("<<q0.toString(3,3)<<")"<<endl;
    cout<<"xd=("<<xd.toString(3,3)<<")"<<endl;
    cout<<"q=("<<q.toString(3,3)<<")"<<endl;
    cout<<endl;

    return 0;
}
コード例 #7
0
ファイル: main.cpp プロジェクト: robotology/cer
    virtual bool configure(ResourceFinder &rf)
    {
        string slash = "/";
        string ctrlName;
        string robotName;
       // string remoteName;
        string localName;

        Time::turboBoost();

        // get params from the RF
        ctrlName = rf.check("local", Value("robotJoystickControl")).asString();
        robotName = rf.check("robot", Value("cer")).asString();

        localName = "/" + ctrlName;

        //reads the configuration file
        Property ctrl_options;

        ConstString configFile = rf.findFile("from");
        if (configFile == "") //--from torsoJoystickControl.ini
        {
            yWarning("Cannot find .ini configuration file. By default I'm searching for torsoJoystickControl.ini");
            //return false;
        }
        else
        {
            ctrl_options.fromConfigFile(configFile.c_str());
        }

        ctrl_options.put("robot", robotName.c_str());
        ctrl_options.put("local", localName.c_str());

        //check for robotInterface availablity
        yInfo("Checking for robotInterface availability");
        Port startport;
        startport.open(localName+"/robotInterfaceCheck:rpc");


        Bottle cmd; cmd.addString("is_ready");
        Bottle response;
        int rc_count = 0;
        int rp_count = 0;
        int rf_count = 0;
        double start_time = yarp::os::Time::now();
        bool not_yet_connected = true;

        bool skip_robot_interface_check = rf.check("skip_robot_interface_check");
        if (skip_robot_interface_check)
        {
            yInfo("skipping robotInterface check");
        }
        else
        {
            do
            {
                if (not_yet_connected)
                {
                    bool rc = yarp::os::Network::connect(localName + "/robotInterfaceCheck:rpc", "/" + robotName + "/robotInterface");
                    if (rc == false)
                    {
                        yWarning("Problems trying to connect to RobotInterface %d", rc_count++);
                        yarp::os::Time::delay(1.0);
                        continue;
                    }
                    else
                    {
                        not_yet_connected = false;
                        yDebug("Connection established with robotInterface");
                    }
                }

                bool rp = startport.write(cmd, response);
                if (rp == false)
                {
                    yWarning("Problems trying to connect to RobotInterface %d", rp_count++);
                    if (yarp::os::Time::now() - start_time > 30)
                    {
                        yError("Timeout expired while trying to connect to robotInterface");
                        return false;
                    }
                    yarp::os::Time::delay(1.0);
                    continue;
                }
                else
                {
                    if (response.get(0).asString() != "ok")
                    {
                        yWarning("RobotInterface is not ready yet, retrying... %d", rf_count++);
                        if (yarp::os::Time::now() - start_time > 30)
                        {
                            yError("Timeout expired while waiting for robotInterface availability");
                            return false;
                        }
                        yarp::os::Time::delay(1.0);
                        continue;
                    }
                    else
                    {
                        yInfo("RobotInterface is ready");
                        break;
                    }
                }
            } while (1);
        }

        //set the thread rate
        int rate = rf.check("rate",Value(10)).asInt();
        yInfo("baseCtrl thread rate: %d ms.",rate);

        //start the control thread
        control_thr = new ControlThread(rate, rf, ctrl_options);
        if (!control_thr->start())
        {
            delete control_thr;
            return false;
        }

        //check for debug mode
        if (rf.check("no_motors"))
        {
            this->control_thr->motors_enabled = false;
            yInfo() << "Motors disabled";
        }

        rpcPort.open((localName+"/rpc").c_str());
        attach(rpcPort);

        return true;
    }
コード例 #8
0
    bool configure(ResourceFinder &rf)
    {
        string robot=rf.check("robot",Value("icub")).asString().c_str();
        string name=rf.check("name",Value("karmaToolFinder")).asString().c_str();
        arm=rf.check("arm",Value("right")).asString().c_str();
        eye=rf.check("eye",Value("left")).asString().c_str();

        if ((arm!="left") && (arm!="right"))
        {
            printf("Invalid arm requested!\n");
            return false;
        }

        if ((eye!="left") && (eye!="right"))
        {
            printf("Invalid eye requested!\n");
            return false;
        }

        Property optionArmL("(device cartesiancontrollerclient)");
        optionArmL.put("remote",("/"+robot+"/cartesianController/left_arm").c_str());
        optionArmL.put("local",("/"+name+"/left_arm").c_str());
        if (!drvArmL.open(optionArmL))
        {
            printf("Cartesian left_arm controller not available!\n");
            terminate();
            return false;
        }

        Property optionArmR("(device cartesiancontrollerclient)");
        optionArmR.put("remote",("/"+robot+"/cartesianController/right_arm").c_str());
        optionArmR.put("local",("/"+name+"/right_arm").c_str());
        if (!drvArmR.open(optionArmR))
        {
            printf("Cartesian right_arm controller not available!\n");
            terminate();
            return false;
        }

        if (arm=="left")
            drvArmL.view(iarm);
        else
            drvArmR.view(iarm);

        Property optionGaze("(device gazecontrollerclient)");
        optionGaze.put("remote","/iKinGazeCtrl");
        optionGaze.put("local",("/"+name+"/gaze").c_str());
        if (drvGaze.open(optionGaze))
            drvGaze.view(igaze);
        else
        {
            printf("Gaze controller not available!\n");
            terminate();
            return false;
        }

        Bottle info;
        igaze->getInfo(info);
        if (Bottle *pB=info.find(("camera_intrinsics_"+eye).c_str()).asList())
        {
            int cnt=0;
            Prj.resize(3,4);
            for (int r=0; r<Prj.rows(); r++)
                for (int c=0; c<Prj.cols(); c++)
                    Prj(r,c)=pB->get(cnt++).asDouble();
        }
        else
        {
            printf("Camera intrinsic parameters not available!\n");
            terminate();
            return false;
        }

        imgInPort.open(("/"+name+"/img:i").c_str());
        imgOutPort.open(("/"+name+"/img:o").c_str());
        dataInPort.open(("/"+name+"/in").c_str());
        logPort.open(("/"+name+"/log:o").c_str());
        rpcPort.open(("/"+name+"/rpc").c_str());
        attach(rpcPort);

        Vector min(3),max(3);
        min[0]=-1.0; max[0]=1.0;
        min[1]=-1.0; max[1]=1.0;
        min[2]=-1.0; max[2]=1.0;
        solver.setBounds(min,max);
        solution.resize(3,0.0);

        enabled=false;
        dataInPort.setReader(*this);

        return true;
    }
コード例 #9
0
bool scriptModule::configure(ResourceFinder &rf) {
    Time::turboBoost();
    
    this->rfCopy = rf;

    if (rf.check("name"))
        name=string("/")+rf.find("name").asString().c_str();
    else
        name="/walkPlayer";

    contextPath=rf.getContextPath().c_str();
    fprintf(stderr,"||| contextPath: %s\n", contextPath.c_str());

    rpcPort.open((name+"/rpc").c_str());
    attach(rpcPort);

    Property portProp;
    portProp.put("robot", rf.find("robot"));

    //*** start the robot driver
    if (!robot.configure(portProp))
    {
        cerr<<"Error configuring position controller, check parameters"<<endl;
        return false;
    }
    else {
        cout << "Configuration done." << endl;
    }

    if (!robot.init())
    {
        cerr<<"Error cannot connect to remote ports"<<endl;
        return false;
    }
    else {
        cout << "Initialization done." << endl;
    }

    //*** attach the robot driver to the thread and start it
    thread.attachRobotDriver(&robot);
    if (!thread.start())
    {
        cerr<<"ERROR: Thread did not start, queue will not work"<<endl;
    }
    else
    {
        cout<<"Thread started"<<endl;
    }

    if (rf.check("execute")==true)
    {
        cout << "Enablig iPid->setReference() controller"<< endl;
        thread.enable_execute_joint_command = true;
    }
    else
    {
        cout << "Not using iPid->setReference() controller"<< endl;
        thread.enable_execute_joint_command = false;
    }

    if (rf.check("period")==true)
    {
        int period = rf.find("period").asInt();
        cout << "Thread period set to "<<period<< "ms" <<endl;
        thread.setRate(period);
    }

    if (rf.check("speed")==true)
    {
        double fact = rf.find("speed").asDouble();
        cout << "speed factor set to "<<fact<< endl;
        thread.speed_factor= fact;
    }

    //*** open the position file
    cout << "opening file..." << endl;

    if (rf.check("filename")==true)
    {
        string filename = rf.find("filename").asString().c_str();
        if (!thread.actions.openFile(filename, rf))
        {
            cout << "ERROR: Unable to parse file" << endl;
            return false;
        };
    }
    
    if (rf.check("minJerkLimit")==true )
    {
        int tmpLimit = rf.find("minJerkLimit").asInt();
        thread.minJerkLimit = tmpLimit;
    }
    
    if (rf.check("refSpeedMinJerk")==true )
    {
        thread.refSpeedMinJerk = rf.find("refSpeedMinJerk").asDouble();
    } else {
        thread.refSpeedMinJerk = 0.0;
    }
    
    if (rf.check("torqueBalancingSequence")==true)
    {
        string filenamePrefix = rf.find("torqueBalancingSequence").asString().c_str();
        // Overwrite the execute flag value. This option has higher priority and
        // should simply stream trajectories use by the torqueBalancing module.
        thread.enable_execute_joint_command = true;
        if (!thread.actions.openTorqueBalancingSequence(filenamePrefix,rf))
        {
            cout << "ERROR: Unable to parse torque balancing sequence" << endl;
            return false;
        }
    }

    cout << "Using parameters:" << endl << rf.toString() << endl;
    cout << "module successfully configured. ready." << endl;
    return true;
}
コード例 #10
0
ファイル: main.cpp プロジェクト: asilx/rossi-demo
int
main (int argc, char *argv[])
{
  Network yarp;

  if (!yarp.checkNetwork ())
    return -1;

  YARP_REGISTER_DEVICES(icubmod)

  ResourceFinder rf;
  rf.setVerbose (true);
  rf.setDefaultContext ("actionPrimitivesExample/conf");
  rf.setDefaultConfigFile ("config.ini");
  rf.setDefault ("part", "left_arm");
  rf.setDefault ("grasp_model_type", "tactile");
  rf.setDefault ("grasp_model_file", "grasp_model.ini");
  rf.setDefault ("hand_sequences_file", "hand_sequences.ini");
  rf.setDefault ("name", "actionPrimitivesMod");
  rf.setDefault ("sim", "off");
  rf.configure ("ICUB_ROOT", argc, argv);

  BehaviorModule mod;

  return mod.runModule (rf);
}
コード例 #11
0
ファイル: lrh.cpp プロジェクト: MagnusJohnsson/wysiwyd
bool LRH::configure(ResourceFinder &rf) {

    bool	bEveryThingisGood = true;
    moduleName = rf.check("name",
        Value("lrh"),
        "module name (string)").asString();

    sKeyWord = rf.check("keyword", Value("grammar")).toString().c_str();
    cout << "**************Context path for grammars: " << rf.getContextPath() << endl;

    /* Mode Action Performer => Meaning*/
    scorpusFileAP = rf.getContextPath().c_str();
    scorpusFileAP += rf.check("corpusFileAP", Value("/Corpus/corpus_AP.txt")).toString().c_str();
    scorpusFileSD = rf.getContextPath().c_str();
    scorpusFileSD += rf.check("corpusFileSD", Value("/Corpus/corpus_SD.txt")).toString().c_str();
    sfileResult = rf.getContextPath().c_str();
    sfileResult += rf.check("fileResult", Value("/Corpus/output.txt")).toString().c_str();
    stemporaryCorpus = rf.getContextPath().c_str();
    stemporaryCorpus += rf.check("temporaryCorpus", Value("/Corpus/temporaryCorpus.txt")).toString().c_str();
    sreservoirAP = rf.getContextPath().c_str();
    sreservoirAP += rf.check("fileAP", Value("/action_performer_PAOR.py")).toString().c_str();
    cout << "sreservoirAP : " << sreservoirAP << endl;
    sreservoirSD = rf.getContextPath().c_str();
    sreservoirSD += rf.check("fileAP", Value("/spatial_relation.py")).toString().c_str();
    sclosed_class_wordsAP = rf.check("closed_class_wordsSD", Value("after")).toString().c_str();
    sclosed_class_wordsSD = rf.check("closed_class_wordsSD", Value("after")).toString().c_str();
    smax_nr_ocw = rf.check("max_nr_ocw", Value("10")).toString().c_str();
    smax_nr_actionrelation = rf.check("max_nr_actionrelation", Value("4")).toString().c_str();
    selt_pred = rf.check("elt_pred", Value("P A O R V")).toString().c_str();

    sHand = rf.check("hand", Value("right")).toString().c_str();
    offsetGrasp = rf.check("offsetGrasp", Value("0.02")).asDouble();

    setName(moduleName.c_str());

    // Open handler port
    string sName = getName();
    handlerPortName = "/" + sName + "/rpc";

    if (!handlerPort.open(handlerPortName.c_str())) {
        cout << getName() << ": Unable to open port " << handlerPortName << endl;
        bEveryThingisGood = false;
    }

    attach(handlerPort);                  // attach to port

    //------------------------//
    //		iCub Client
    //------------------------//

    // string ttsSystem = SUBSYSTEM_SPEECH;
    iCub = new ICubClient(moduleName.c_str(), "lrh", "client.ini", true);
    iCub->opc->isVerbose = false;

    char rep = 'n';
    while (rep != 'y'&&!iCub->connect())
    {
        cout << "iCubClient : Some dependencies are not running..." << endl;
        break; //to debug
        Time::delay(1.0);
    }
    cout << "Connections done" << endl;
    iCub->opc->checkout();
    cout << "Checkout done" << endl;

    //populateOPC();

    return true;
}
コード例 #12
0
ファイル: Demo1Main.cpp プロジェクト: xufango/contrib_bk
int main()
{
	Network::init();

	// open an rpc interface to receive user's commands
	Port speaker;
	speaker.setRpcMode(true);
	int i=0;
	while(!speaker.open("/IITdemo/rpc"))
	{
		if(i>=4) {cout << "Error in creating rpc port" << endl; Network::fini(); return -1;}
		Time::delay(0.25);
		i++;
	}

	// open a face interface to dialog with expressions
	Port face;
	while(!face.open("/IITdemo/face:o"))
	{
		if(i>=4) {cout << "Error in creating face port" << endl; speaker.close(); Network::fini(); return -1;}
		Time::delay(0.25);
		i++;
	}

	// open a planner instance that executes user's commands
	Demo1Module * planner;
	ResourceFinder rf;
	rf.setVerbose(true); // print to a console conf info
	rf.setDefaultConfigFile("C:/DARWIN/ConXML/conf/Demo1Configuration.ini");
	rf.setDefaultContext("../conf"); // dove sono i file .ini
	rf.configure("ICUB_ROOT",0,NULL);

	planner = new Demo1Module(rf,20);
	if(!planner->threadInit())
	{
		fprintf(stderr, "Error configuring module returning\n");
		face.close();
		return -1;
	}

	if(!planner->start())
	{
		planner->threadRelease();
		fprintf(stderr, "Error configuring module returning\n");
		face.close();
		return -1;
	}

	// wait for the port to be connected

	//send a welcome message:

	Network::connect(face.getName().c_str(),"/icub/face/emotions/in");
	Bottle faceCmd,test,reply;
	test.clear();
	faceCmd.append("set mou sur");
	face.write(faceCmd,test);

	Bottle welcome;
	welcome.addString("--> Hello! Are you ready to play with me?");
		
	speaker.read(test,true);
	reply.append(welcome);
	speaker.reply(reply);

	test.clear();
	speaker.read(test,true);
	
	if (test.get(0).asString() == "no") 
	{
		faceCmd.clear();
		faceCmd.append("set mou sad");
		face.write(faceCmd,test);

		welcome.clear();
		welcome.addString("--> Bye");
		speaker.replyAndDrop(welcome);

		planner->stop();
		speaker.close();
		face.close();
		Network::fini(); 
		return 0;
	}
	
	faceCmd.clear();
	faceCmd.append("set all hap");
	face.write(faceCmd,test);

	// mettere icub in posizione relax
	planner->Relax();
	if(!planner->startPMP())
	{
		welcome.clear();
		welcome.addString("--> Did you connect my ports?");
		speaker.reply(welcome);
	}

	planner->enableTouch();

	planner->suspend();
	welcome.clear();
	welcome.addString("--> What should I do?");
	speaker.reply(welcome);
	
	Bottle cmd;

	while(true)
	{
		reply.clear();
		speaker.read(cmd,true);
		if (!planner->arePortsConnected() && cmd.get(0).asString() != "quit")
		{
			reply.append("--> Have you connected all my ports?");
			speaker.reply(reply);
		}
		else
		{
			string first = cmd.get(0).asString().c_str();
		
			if (first == "stop")
			{
				if(planner->stopMotion()) reply.addString("--> OK, I'm freezed");
				else					  reply.addString("--> Sorry, I'm not freezed");
				speaker.reply(reply);
			}
			else if(first == "save")
			{
				int done;
				if(cmd.size() != 5 || !cmd.get(4).isString())
					done = -1;
				else
					done = planner->add2map(cmd.get(4).asString().c_str(),cmd.get(1).asDouble(),cmd.get(2).asDouble(),cmd.get(3).asDouble());
				switch(done)
				{ 
				case 1:		reply.addString("--> Ok, I got it! But it was too low, I have saved it higher");break;
				case 0:		reply.addString("--> Ok, now I know this object!"); break;
				case -1:	reply.addString("--> Sorry, I can't understand");break;
				case -2:	reply.addString("--> Hey, I already knew this object!"); break;
				}
				speaker.reply(reply);
			}

			else if(first == "userRecord")
			{
				int done;
				bool left = (cmd.get(1).asString() == "left");

				// record using user input to stop
				if (cmd.size() == 2)
				{
					if(!left)
						done = planner->userRecordStart(false);
					else
						done = planner->userRecordStart();
				}

				if (done == 0)
				{
					reply.addString("--> Ok, you can guide me!");
					speaker.reply(reply);
					Bottle stop;
					speaker.read(stop,true);

					if(!left)
						done = planner->userRecordStop(false);
					else
						done = planner->userRecordStop();
				}

				switch(done)
				{
				case 1: 
						reply.addString("--> Ok, I got it! But it was too low, I have saved it higher. How is this object called (label <name>)?");
						break;
				case 0: 
						reply.addString("--> Ok, I got it! How is this object called (label <name>)?");
						break;
				case -3: reply.addString("--> Sorry, I did not understand what you said");break;
				case -4: reply.addString("--> Sorry, I could not stop my motion");break;
				case -5: reply.addString("--> Sorry, I could not start recording");break;
				case -6: reply.addString("--> Sorry, I don't know where my arm is");break;
				}

			}
			else if(first == "record")
			{
				int done;

				// record using user input to stop
				if (cmd.size() == 2)
				{
					if(cmd.get(1).asString() == "right")
						done = planner->Record(false);
					else if(cmd.get(1).asString() == "left")
						done = planner->Record();
					else
						done = -3;
				}
				else
					done = planner->Record();
				
				switch(done)
				{
				case 1: 
						// wait for the point to be recorded: if z<=SAFETY_Z, z = z+0.1
						// once it has been recorded ask for the object label
						planner->pointRecorded.wait();
						Time::delay(0.5);
						//planner->Relax();
						planner->Prepare();
						reply.addString("--> Ok, I got it! But it was too low, I have saved it higher. How is this object called (label <name>)?");
						break;
				case 0: 
						// wait for the point to be recorded
						// once it has been recorded ask for the object label
						planner->pointRecorded.wait();
						Time::delay(5);
						//planner->Relax();
						planner->Prepare();
						reply.addString("--> Ok, I got it! How is this object called (label <name>)?");
						break;
				case -3: reply.addString("--> Sorry, I did not understand what you said");break;
				case -4: reply.addString("--> Sorry, I could not stop my motion");break;
				case -5: reply.addString("--> Sorry, I could not start recording");break;
				case -6: reply.addString("--> Sorry, I don't know where my arm is");break;
				}
		
				speaker.reply(reply);
			}
			else if (first == "label")
			{
				if (cmd.size() != 2)
					reply.addString("--> Sorry, I did not understand the label");
				else
				{
					int done = planner->Label(cmd.get(1).asString().c_str());
					switch(done)
					{ 
					case 0:		reply.addString("--> Ok, now I know this object!"); break;
					case -1:	reply.addString("--> Sorry, I don't have any point to label"); break;
					case -2:	reply.addString("--> Hey, I already knew this object!"); break;
					}
				}
				speaker.reply(reply);
			}

			else if (first == "forget")
			{
				if (cmd.size() != 2)
					reply.addString("--> Sorry, I did not understand the label");
				else
				{
					int done = planner->Forget(cmd.get(1).asString().c_str());
					switch(done)
					{ 
					case 0:		reply.addString("--> Ok, object forgotten!"); break;
					case -1:	reply.addString("--> Sorry, I don't remember this object"); break;
					}
				}
				speaker.reply(reply);
			}

			else if (first == "recall")
			{
				reply = planner->Recall();
				if (reply.size() == 0)
					reply.addString("--> Sorry, tabula rasa");
				speaker.reply(reply);
			}

			else if (first == "relax")
			{
				int done = planner->Relax();
				switch(done)
					{ 
					case 0:		reply.addString("--> Ok, I relax!"); break;
					case -1:	reply.addString("--> Sorry, I can't relax"); break;
					}
				speaker.reply(reply);
			}

			else if (first == "prepare")
			{
				int done = planner->Prepare();
				switch(done)
					{ 
					case 0:		reply.addString("--> Ok, I'm ready!"); break;
					case -1:	reply.addString("--> Sorry, something went wrong"); break;
					}
				speaker.reply(reply);
			}
			// sequence reaching
			else if (first == "reach")
			{
				if(cmd.size() == 1)
					reply.addString("--> Sorry, I don't know what I should reach with my finger");
				else
				{
					int done = planner->SequenceReach(cmd.tail());
					switch(done)
					{
					case 0: reply.addString("--> Ok, I have reached the object!"); break;
					case -1: reply.addString("--> Sorry, I don't know this object"); break;
					case -2: reply.addString("--> Sorry, I don't know where my hands are"); break;
					case -3: reply.addString("--> Sorry, I did not understand what you said"); break;
					case -4: reply.addString("--> Sorry, my ports are not connected"); break;
					case -5: reply.addString("--> Sorry, I don't know this command"); break;
					case -8: reply.addString("--> Sorry, I couldn't reach the object"); break;
					case -9: reply.addString("--> Sorry, my wrist orientation is wrong"); break;
					}
				}
				speaker.reply(reply);
			}

			// finger reaching
			else if (first == "reach1")
			{
				if(cmd.size() != 3 && cmd.size() != 2)
					reply.addString("--> Sorry, I don't know what I should reach with my finger");
				else
				{
					
					int done = planner->Prepare();
					if(done != 0) reply.addString("--> Sorry, I'm lazy");
					else if (cmd.size() == 3)
					{
						if (cmd.get(2).asString() == "top" || cmd.get(2).asString() == "side")
						{
							done = planner->FingerReach(cmd.get(1).asString().c_str(),"",cmd.get(2).asString().c_str(),false);
							if (done == 0)
							{
								cout << "check reaching performance" << endl;
								done = planner->checkReachingSuccess(cmd.get(1).asString().c_str());
							}
						}
						else
							done = -9;
					}						
					else
					{
						done = planner->FingerReach(cmd.get(1).asString().c_str(),"","top",false);
						if (done == 0)
						{
							cout << "check reaching performance" << endl;
							done = planner->checkReachingSuccess(cmd.get(1).asString().c_str());
						}
					}

					switch(done)
					{
					case 0: 
						reply.addString("--> Ok, I have reached the object!");
						//done = planner->Lift(planner->activeChain);
						//if (done !=0 )
						//{reply.addString("But I could not lift my hand"); cout << "done: " << done << endl;break;}
						done = planner->goFar(planner->activeChain);
						if (done !=0 )
						{reply.addString("But I could not go farer"); cout << "done: " << done << endl;}
						planner->Relax();
						break;
					case -1: reply.addString("--> Sorry, I don't know this object"); break;
					case -2: reply.addString("--> Sorry, I don't know where my hands are"); break;
					case -3: reply.addString("--> Sorry, I did not understand what you said"); break;
					case -4: reply.addString("--> Sorry, my ports are not connected"); break;
					case -5: reply.addString("--> Sorry, I don't know this command"); break;
					case -8: reply.addString("--> Sorry, I couldn't reach the object"); break;
					case -9: reply.addString("--> Sorry, my wrist orientation is wrong"); break;
					}
				}

				speaker.reply(reply);
			}
			// reach to grasp function
			else if (first == "grasp")
			{
				if(cmd.size() != 2)
				{
					reply.addString("--> Sorry, I don't know what I should grasp");
				}
				else
				{
					cout << "prepare for grasping" << endl;
					// this function will return 1 if no reaching is needed, 0 otherwise
					int done = planner->prepare4Grasping(cmd.get(1).asString().c_str());
					if (done == 0)
					{
						// check that motion is finished successfully
						cout << "check reaching performance" << endl;
						done = planner->checkReachingSuccess(cmd.get(1).asString().c_str());
					}
					if(done == 0 || done == 1)
					{
						cout << "grasp" << endl;
						done = (planner->Grasp()).get(0).asInt();
					}
					if(done == 0)
					{
						done = planner->Lift(planner->activeChain,true);
						// check if contact is mantained
						if(done == 0)
						{
							done = planner->Release(planner->activeChain);
							if (done == 0)				// reinitialize
								done = planner->Relax();
						}
					}
					switch(done)
					{
					case 0:
					case 1:  reply.addString("--> Ok, I grasped the object!"); break;
					case -1: reply.addString("--> Sorry, I don't know this object"); break;
					case -2: reply.addString("--> Sorry, I don't know where my hands are"); break;
					case -3: reply.addString("--> Sorry, I did not understand what you said"); break;
					case -4: reply.addString("--> Sorry, my ports are not connected, I can't grasp"); break;
					case -5: reply.addString("--> Sorry, I don't know this command"); break;
					case -6: reply.addString("--> Sorry, I don't have this hand"); break;
					case -7: reply.addString("--> Sorry, I couldn't grasp the object"); break;
					case -8: reply.addString("--> Sorry, I couldn't reach the object"); break;
					case -9: reply.addString("--> Hey, I have lost the object!"); break;
					}
				}
				speaker.reply(reply);
			}

			else if (first == "release")
			{
				if(cmd.size() != 2)
					reply.addString("--> Sorry, I don't know what hand to open");
				else
				{
					int done = planner->Release(cmd.get(1).asString().c_str());
					switch(done)
						{ 
						case 0:		reply.addString("--> Ok, I released the object!"); break;
						case -1:	reply.addString("--> Sorry, I can't release the object"); break;
						}
					if (done == 0)	planner->Relax();//planner->initIcubUp();
				}
				speaker.reply(reply);
			}

			else if (first == "lift")
			{
				if(cmd.size() != 2)
					reply.addString("--> Sorry, I don't know what hand to lift");
				else
				{
					int done = planner->Lift(cmd.get(1).asString().c_str());
					switch(done)
					{		
					case 0:  reply.addString("--> Ok, I have lifted my hand!"); break;
					case -2: reply.addString("--> Sorry, I don't know where my hands are"); break;
					case -3: reply.addString("--> Sorry, I did not understand what you said"); break;
					case -4: reply.addString("--> Sorry, my ports are not connected"); break;
					case -5: reply.addString("--> Sorry, I don't know this command"); break;
					case -6: reply.addString("--> Sorry, I don't have this hand"); break;
					case -9: reply.addString("--> Hey, I have lost the object!"); break;
					}
					
				}

				speaker.reply(reply);
			}

			else if (first == "carry")
			{
				if(cmd.size() != 2)
				{
					reply.addString("--> Sorry, I don't know what I should grasp");
				}
				else
				{
					

				}
				speaker.reply(reply);
			}
			else if (first == "unstack")
			{
				int done = planner->Unstack();

					switch(done)
					{		
					case 0:  reply.addString("--> Ok, I have recycled the object!"); break;
					case -1: reply.addString("--> Sorry, I don't know this object"); break;
					case 1 : reply.addString("--> Sorry, I don't know where the litter is (label = bin)"); break;
					case -2: reply.addString("--> Sorry, I don't know where my hands are"); break;
					case -3: reply.addString("--> Sorry, I did not understand what you said"); break;
					case -4: reply.addString("--> Sorry, my ports are not connected"); break;
					case -5: reply.addString("--> Sorry, I don't know this command"); break;
					case -6: reply.addString("--> Sorry, I don't have this hand"); break;
					case -7: reply.addString("--> Sorry, I couldn't grasp the object"); break;
					case -8: reply.addString("--> Sorry, I couldn't reach the object"); break;
					case -9: reply.addString("--> Sorry, my wrist orientation is wrong"); break;
					case -10: reply.addString("--> Hey, I have lost the object!"); break;
					case -11: reply.addString("--> Hey, I do not see a stack!"); break;
					case -12: reply.addString("--> Sorry, points are not consistent!"); break;
					case -13: reply.addString("--> Sorry, I can't see any object!"); break;
					}
					speaker.reply(reply);
					planner->Prepare();
			}

			else if (first == "recycle")
			{
				if(cmd.size() != 2)
				{
					reply.addString("--> Sorry, I don't know what I should throw in the garbage");
				}
				else
				{
					int done = planner->Recycle(cmd.get(1).asString().c_str());

					switch(done)
					{		
					case 0:  reply.addString("--> Ok, I have recycled the object!"); break;
					case -1: reply.addString("--> Sorry, I don't know this object"); break;
					case 1 : reply.addString("--> Sorry, I don't know where the litter is (label = bin)"); break;
					case -2: reply.addString("--> Sorry, I don't know where my hands are"); break;
					case -3: reply.addString("--> Sorry, I did not understand what you said"); break;
					case -4: reply.addString("--> Sorry, my ports are not connected"); break;
					case -5: reply.addString("--> Sorry, I don't know this command"); break;
					case -6: reply.addString("--> Sorry, I don't have this hand"); break;
					case -7: reply.addString("--> Sorry, I couldn't grasp the object"); break;
					case -8: reply.addString("--> Sorry, I couldn't reach the object"); break;
					case -9: reply.addString("--> Sorry, my wrist orientation is wrong"); break;
					case -10: reply.addString("--> Hey, I have lost the object!"); break;
					}
					planner->Relax();
				}
				speaker.reply(reply);
			}
			else if (first == "geometry")
			{
				if(cmd.size() < 2)
					reply.addString("--> Sorry, I don't understand");

				if(cmd.get(1).asString() == "get")
					reply.append(planner->getObjectGeometry());
				else if (cmd.get(1).asString() == "set" && cmd.size() == 4)
				{
					planner->setObjectGeometry(cmd.get(2).asDouble(),cmd.get(3).asDouble());
					reply.addString("--> OK, now I have new geometry values");
				}
				else
					reply.addString("--> Sorry, I don't understand");

				speaker.reply(reply);
			}
			else if (first == "bimanual")
			{
				if(cmd.size() < 2)
					reply.addString("--> Sorry, I don't know what I should do with my hands");
				else
				{
					int done;
					if (cmd.get(1).asString() == "reach")
					{
						switch(cmd.size())
						{
						case 3: // use the geometrical info abt the object to reach it
							done = planner->initBimanualReach(cmd.get(2).asString().c_str(),cmd.get(2).asString().c_str());
							planner->BimanualRelease();
							break;
						case 4: // reach from top two different objects
							done = planner->initBimanualReach(cmd.get(2).asString().c_str(),cmd.get(3).asString().c_str());
							break;
						}
						// get the label of the object/s to reach: if one 
					}
					else if (cmd.get(1).asString() == "grasp" && cmd.size() == 3)
					{
						done = planner->initBimanualReach(cmd.get(2).asString().c_str(),cmd.get(2).asString().c_str());
						if (done == 0)
						{
							done = planner->checkBimanualReachingSuccess(cmd.get(2).asString().c_str(),cmd.get(2).asString().c_str());
							if (done == 0)
							{
								done = planner->BimanualGrasp();
								if (done == 0)
									done = planner->BimanualLift(false);
							}
						}

						Time::delay(1);
						planner->BimanualRelease();

					}
					else if(cmd.get(1).asString() == "carry")
					{
					}

					switch(done)
					{		
					case 0:  reply.addString("--> Ok, I have recycled the object!"); break;
					case -1: reply.addString("--> Sorry, I don't know this object"); break;
					case 1 : reply.addString("--> Sorry, I don't know where the litter is (label = bin)"); break;
					case -2: reply.addString("--> Sorry, I don't know where my hands are"); break;
					case -3: reply.addString("--> Sorry, I did not understand what you said"); break;
					case -4: reply.addString("--> Sorry, my ports are not connected"); break;
					case -5: reply.addString("--> Sorry, I don't know this command"); break;
					case -6: reply.addString("--> Sorry, I don't have this hand"); break;
					case -7: reply.addString("--> Sorry, I couldn't grasp the object"); break;
					case -8: reply.addString("--> Sorry, I couldn't reach the object"); break;
					case -9: reply.addString("--> Sorry, my wrist orientation is wrong"); break;
					case -10: reply.addString("--> Hey, I have lost the object!"); break;
					}
				}
				speaker.reply(reply);
			}

			else if (first == "quit")
			{
				planner->Relax();
				//if(planner->isSuspended())	planner->resume();
				planner->stop();
				reply.addString("--> Bye");
				speaker.replyAndDrop(reply);
				break;
			}
			else
			{
				reply.addString("--> I did not understand what you said");
				speaker.reply(reply);
			}
		}

		cmd.clear();
		reply.clear();
	}

	face.interrupt();
	face.close();
	speaker.close();
	Network::fini();

	return 0;
}
コード例 #13
0
ファイル: Drivers.cpp プロジェクト: claudiofantacci/yarp
int Drivers::yarpdev(int argc, char *argv[]) {

    std::signal(SIGINT, handler);
    std::signal(SIGTERM, handler);

    // get command line options
    ResourceFinder rf;
    rf.configure(argc, argv); // this will process --from FILE if present
    Property options;

    // yarpdev will by default try to pass its thread on to the device.
    // this is because some libraries need configuration and all
    // access methods done in the same thread (e.g. opencv_grabber
    // apparently).
    options.put("single_threaded", 1);

    // interpret command line options as a set of flags
    //options.fromCommand(argc,argv,true,false);
    options.fromString(rf.toString().c_str(), false);

    // check if we're being asked to read the options from file
    Value *val;
    if (options.check("file",val))
    {
        // FIXME use argv[0]
        yError("*** yarpdev --file is deprecated, please use --from\n");
        yError("*** yarpdev --file will be removed in a future version of YARP\n");

        std::string fname = val->toString();
        options.unput("file");
        yDebug("yarpdev: working with config file %s\n", fname.c_str());
        options.fromConfigFile(fname,false);

        // interpret command line options as a set of flags again
        // (just in case we need to override something)
        options.fromCommand(argc,argv,true,false);
    }

    // check if we want to use nested options (less ambiguous)
    if (options.check("nested",val)||options.check("lispy",val))
    {
        std::string lispy = val->toString();
        yDebug("yarpdev: working with config %s\n", lispy.c_str());
        options.fromString(lispy);
    }

    if (!options.check("device"))
    {
        // no device mentioned - maybe user needs help
        if (options.check("list"))
        {
            yInfo("Here are devices listed for your system:");
            for (auto& s : split(Drivers::factory().toString(), '\n')) {
                yInfo("%s", s.c_str());
            }
        }
        else
        {
            yInfo("Welcome to yarpdev, a program to create YARP devices\n");
            yInfo("To see the devices available, try:\n");
            yInfo("   yarpdev --list\n");
            yInfo("To create a device whose name you know, call yarpdev like this:\n");
            yInfo("   yarpdev --device DEVICENAME --OPTION VALUE ...\n");
            yInfo("   For example:\n");
            yInfo("   yarpdev --device test_grabber --width 32 --height 16 --name /grabber\n");
            yInfo("You can always move options to a configuration file:\n");
            yInfo("   yarpdev [--device DEVICENAME] --from CONFIG_FILENAME\n");
            yInfo("If you have problems, you can add the \"verbose\" flag to get more information\n");
            yInfo("   yarpdev --verbose --device ffmpeg_grabber\n");
            if (options.check ("from"))
            {
                yError("Unable to find --device option in file %s. Closing.", options.find("from").asString().c_str());
            }
            else
            {
                yWarning("--device option not specified. Closing.");
            }
        }
        return 0;
    }

    // ask for a wrapped, remotable device rather than raw device
    options.put("wrapped","1");

    //YarpDevMonitor monitor;
    bool verbose = false;
    if (options.check("verbose")) {
        verbose = true;
        //options.setMonitor(&monitor,"top-level");
    }

    // we now need network
    bool ret=Network::checkNetwork();
    if (!ret)
    {
        yError("YARP network not available, check if yarp server is reachable\n");
        return -1;
    }

    //
    // yarpdev initializes the clock only before starting to do real thing.
    // This way yarpdev --lish/help will not be affected by network clock.
    //
    // Shall other devices be affected by network clock ??
    // Hereafter the device may need to use the SystemClock or the NetworkClock
    // depending by the device, a real or a fake / simulated one.
    // Using the YARP_CLOCK_DEFAULT the behaviour will be determined by the
    // environment variable.
    //
    yarp::os::NetworkBase::yarpClockInit(yarp::os::YARP_CLOCK_DEFAULT);

    PolyDriver dd(options);
    if (verbose) {
        toDox(dd,stdout);
    }
    if (!dd.isValid()) {
        yError("yarpdev: ***ERROR*** device not available.\n");
        if (argc==1)
        {
            yInfo("Here are the known devices:\n");
            yInfo("%s", Drivers::factory().toString().c_str());
        }
        else
        {
            yInfo("Suggestions:\n");
            yInfo("+ Do \"yarpdev --list\" to see list of supported devices.\n");
            if (!options.check("verbose"))
            {
                yInfo("+ Or append \"--verbose\" option to get more information.\n");
            }
        }
        return 1;
    }

    Terminee *terminee = nullptr;
    if (dd.isValid()) {
        Value *v;
        std::string s("/yarpdev/quit");
        if (options.check("device", v)) {
            if (v->isString()) {
                s = "";
                s += "/";
                s += v->toString();
                s += "/quit";
            }
        }
        if (options.check("name", v)) {
            s = "";
            s += v->toString();
            s += "/quit";
        }
        if (s.find("=") == std::string::npos &&
            s.find("@") == std::string::npos) {
            terminee = new Terminee(s.c_str());
            terminatorKey = s.c_str();
            if (terminee == nullptr) {
                yError("Can't allocate terminator port\n");
                terminatorKey = "";
                dd.close();
                return 1;
            }
            if (!terminee->isOk()) {
                yError("Failed to create terminator port\n");
                terminatorKey = "";
                delete terminee;
                terminee = nullptr;
                dd.close();
                return 1;
            }
        }
    }

    double dnow = 3;
    double startTime = Time::now()-dnow;
    IService *service = nullptr;
    dd.view(service);
    if (service!=nullptr) {
        bool backgrounded = service->startService();
        if (backgrounded) {
            // we don't need to poll this, so forget about the
            // service interface
            yDebug("yarpdev: service backgrounded\n");
            service = nullptr;
        }
    }
    while (dd.isValid() && !(terminated||(terminee&&terminee->mustQuit()))) {
        if (service!=nullptr) {
            double now = Time::now();
            if (now-startTime>dnow) {
                yInfo("device active...");
                startTime += dnow;
            }
            // we requested single threading, so need to
            // give the device its chance
            if(!service->updateService()) {
                if(!service->stopService()) {
                    yWarning("Error while stopping device");
                }
                terminated = true;
            }
        } else {
            // we don't need to do anything
            yInfo("device active in background...");
            SystemClock::delaySystem(dnow);
        }
    }

    if (terminee) {
        delete terminee;
        terminee = nullptr;
    }
    dd.close();

    yInfo("yarpdev is finished.");

    return 0;
}
コード例 #14
0
bool learnPrimitive::updateAction(ResourceFinder &rf){
    Bottle bOutput;

    //1. Primitive
    Bottle bAction = rf.findGroup("Action");

    if (!bAction.isNull())
    {
        Bottle * bActionName = bAction.find("ActionName").asList();
        Bottle * bActionArg  = bAction.find("ActionArg").asList();

       if(bActionName->isNull() || bActionArg->isNull()){
           yError() << " [updateAction] : one of the primitiveAction conf is null : ActionName, ActionArg" ;
            return false ;
        }

        int actionSize = -1 ;
        if(bActionName->size() == bActionArg->size()){
            actionSize =  bActionName->size() ;
        } else {
            yError() << " [updateAction] : one of the Action conf has different size!" ;
            return false ;
        }

        if(actionSize == 0) {
            yWarning() << " [updateAction] : there is no Action defined at startup!" ;
        } else {

            for(int i = 0; i < actionSize ; i++) {
                //insert protoaction even if already there
                Bottle bSubAction;
                string currentActionName = bActionName->get(i).asString();
                string currentActionArg  = bActionArg->get(i).asString();

                bSubAction.addString(currentActionName);
                bSubAction.addString(currentActionArg);

                string concat = currentActionName + "_" + currentActionArg;

               // yInfo() << " Looking for " << concat ;

                Bottle bCurrent = rf.findGroup(concat);
                if(bCurrent.isNull()){
                    yError() << " [updateAction] : " << concat << "is NOT defined" ;
                    return false ;
                }

                Bottle * bListSubAction = bCurrent.find("actionList").asList();

                //yInfo() << "Subaction found : " << bListSubAction->toString() ;
                if(bListSubAction->isNull()){
                    yError() << " [updateAction] : " << concat << "is there but is not defined (actionList)" ;
                    return false ;
                }

                bSubAction.addList() = *bListSubAction ;
                yInfo() << "Complex Action added : (" <<bSubAction.get(0).asString() << ", " << bSubAction.get(1).asString() << ", ( " << bSubAction.get(2).asList()->toString()<< "))" ;
                vActionBottle.push_back(bSubAction);
            }
        }
    } else {
        yError() << " error in learnPrimitive::updatePrimitive | Primitive_Action is NOT defined in the learnPrimitive.ini";
        return false;
    }


    return true;
}
コード例 #15
0
int main(int argc, char *argv[])
{ 
  Network yarp;
  ResourceFinder rf;
  BufferedPort<Bottle> inPort, outPort;
  
  rf.setVerbose(true);
  rf.configure(argc,argv);

  if (rf.check("help"))
  {
      usage();
      return 0;
  }
  
  std::string name=rf.check("name",Value("com2gui")).asString();
  std::string wholebody=rf.check("wholebody",Value("wholeBodyDynamics")).asString();
  
  bool ok=inPort.open("/"+name+"/COM:i");
  ok=ok && outPort.open("/"+name+"/objects:o");
  
  if (!ok) 
  {
    yError() << "Could not open yarp ports - is yarpserver running happily ?";
    return EXIT_FAILURE;
  }
  
  yarp.connect(outPort.getName(),"/iCubGui/objects","udp");
  yarp.connect("/"+wholebody+"/com:o",inPort.getName(),"udp");
  
  yInfo() << "Configured: ";
  yInfo() << "  name= " << name;
  yInfo() << "  wholebody= " <<  wholebody << " (reading from /"+wholebody+"/com:o)";
  yInfo() << "  writing to /iCubGui/objects";
  
  int i=0;
  while (true)
  {
    Bottle *in = inPort.read();                                                                                  
    if (in==NULL) {
      yError() << "Failed to read message";
      return EXIT_FAILURE;
    }
    double x=in->get(0).asDouble();
    double y=in->get(1).asDouble();
    double z=in->get(2).asDouble();
    
    Bottle &obj=outPort.prepare();
    obj.clear();
    
    obj.addString("object"); // command to add/update an object
    obj.addString("COM");
    // object dimensions in mm
    // (it will be displayed as an ellipsoid with the tag "COM")
    obj.addDouble(50);
    obj.addDouble(50);
    obj.addDouble(50);
    // object position in millimiters
    // reference frame: X=fwd, Y=left, Z=up
    obj.addDouble(x);
    obj.addDouble(y);
    obj.addDouble(z);
    // object orientation (roll, pitch, yaw) in degrees
    obj.addDouble(0);
    obj.addDouble(0);
    obj.addDouble(0);
    // object color (0-255)
    obj.addInt(255);
    obj.addInt(128);        
    obj.addInt(128);
    // transparency (0.0=invisible 1.0=solid)
    obj.addDouble(1.0);
    outPort.write(true);
  }
  return EXIT_SUCCESS;
}
コード例 #16
0
ファイル: main.cpp プロジェクト: giuliavezzani/react-control
int main(int argc, char *argv[])
{
    ResourceFinder rf;
    rf.configure(argc,argv);

    double sim_time=rf.check("sim-time",Value(10.0)).asDouble();
    double motor_tau=rf.check("motor-tau",Value(0.0)).asDouble();
    string avoidance_type=rf.check("avoidance-type",Value("tactile")).asString();    

    iCubArm arm("left");
    iKinChain &chain=*arm.asChain();
    chain.releaseLink(0);
    chain.releaseLink(1);
    chain.releaseLink(2);

    Vector q0(chain.getDOF(),0.0);
    q0[3]=-25.0; q0[4]=20.0; q0[6]=50.0;
    chain.setAng(CTRL_DEG2RAD*q0);

    Matrix lim(chain.getDOF(),2);
    Matrix v_lim(chain.getDOF(),2);
    for (size_t r=0; r<chain.getDOF(); r++)
    {
        lim(r,0)=CTRL_RAD2DEG*chain(r).getMin();
        lim(r,1)=CTRL_RAD2DEG*chain(r).getMax();
        v_lim(r,0)=-50.0;
        v_lim(r,1)=+50.0;
    }
    v_lim(1,0)=v_lim(1,1)=0.0;  // disable torso roll

    Ipopt::SmartPtr<Ipopt::IpoptApplication> app=new Ipopt::IpoptApplication;
    app->Options()->SetNumericValue("tol",1e-6);
    app->Options()->SetStringValue("mu_strategy","adaptive");
    app->Options()->SetIntegerValue("max_iter",10000);
    app->Options()->SetNumericValue("max_cpu_time",0.05);
    app->Options()->SetStringValue("nlp_scaling_method","gradient-based");
    app->Options()->SetStringValue("hessian_approximation","limited-memory");
    app->Options()->SetStringValue("derivative_test","none");
    app->Options()->SetIntegerValue("print_level",0);
    app->Initialize();

    Ipopt::SmartPtr<ControllerNLP> nlp=new ControllerNLP(chain);

    AvoidanceHandlerAbstract *avhdl;    
    if (avoidance_type=="none")
        avhdl=new AvoidanceHandlerAbstract(arm);
    else if (avoidance_type=="visuo")
        avhdl=new AvoidanceHandlerVisuo(arm);
    else if (avoidance_type=="tactile")
        avhdl=new AvoidanceHandlerTactile(arm);
    else if (avoidance_type=="visuo-tactile")
        avhdl=new AvoidanceHandlerVisuoTactile(arm); 
    else
    {
        yError()<<"unrecognized avoidance type! exiting ...";
        return 1;
    }
    yInfo()<<"Avoidance-Handler="<<avhdl->getType();
    yInfo()<<"Avoidance Parameters="<<avhdl->getParameters().toString();

    double dt=0.01;
    double T=1.0;

    nlp->set_dt(dt);
    Motor motor(q0,lim,motor_tau,dt);
    Vector v(chain.getDOF(),0.0);

    Vector xee=chain.EndEffPosition();
    minJerkTrajGen target(xee,dt,T);
    Vector xc(3);
    xc[0]=-0.35;
    xc[1]=0.0;
    xc[2]=0.1;
    double rt=0.1;

    Vector xo(3);
    xo[0]=-0.3;
    xo[1]=0.0;
    xo[2]=0.4;
    Vector vo(3,0.0);
    vo[2]=-0.1;
    Obstacle obstacle(xo,0.07,vo,dt);

    ofstream fout;
    fout.open("data.log");

    std::signal(SIGINT,signal_handler);
    for (double t=0.0; t<sim_time; t+=dt)
    {
        Vector xd=xc;
        xd[1]+=rt*cos(2.0*M_PI*0.3*t);
        xd[2]+=rt*sin(2.0*M_PI*0.3*t);

        target.computeNextValues(xd);
        Vector xr=target.getPos();

        xo=obstacle.move();

        avhdl->updateCtrlPoints();
        Matrix VLIM=avhdl->getVLIM(obstacle,v_lim);

        nlp->set_xr(xr);
        nlp->set_v_lim(VLIM);
        nlp->set_v0(v);
        nlp->init();
        Ipopt::ApplicationReturnStatus status=app->OptimizeTNLP(GetRawPtr(nlp));
        v=nlp->get_result();

        xee=chain.EndEffPosition(CTRL_DEG2RAD*motor.move(v));

        yInfo()<<"        t [s] = "<<t;
        yInfo()<<"    v [deg/s] = ("<<v.toString(3,3)<<")";
        yInfo()<<" |xr-xee| [m] = "<<norm(xr-xee);
        yInfo()<<"";

        ostringstream strCtrlPoints;
        deque<Vector> ctrlPoints=avhdl->getCtrlPointsPosition();
        for (size_t i=0; i<ctrlPoints.size(); i++)
            strCtrlPoints<<ctrlPoints[i].toString(3,3)<<" ";

        fout<<t<<" "<<
              xr.toString(3,3)<<" "<<
              obstacle.toString()<<" "<<
              v.toString(3,3)<<" "<<
              (CTRL_RAD2DEG*chain.getAng()).toString(3,3)<<" "<<
              strCtrlPoints.str()<<
              endl;

        if (gSignalStatus==SIGINT)
        {
            yWarning("SIGINT detected: exiting ...");
            break;
        }
    }

    fout.close();
    delete avhdl;

    return 0;
}
コード例 #17
0
ファイル: app.cpp プロジェクト: Julien-B/aseprite
// Initializes the application loading the modules, setting the
// graphics mode, loading the configuration and resources, etc.
App::App(int argc, const char* argv[])
  : m_modules(NULL)
  , m_legacy(NULL)
  , m_isGui(false)
  , m_isShell(false)
  , m_exporter(NULL)
{
  ASSERT(m_instance == NULL);
  m_instance = this;

  AppOptions options(argc, argv);

  m_modules = new Modules(!options.startUI(), options.verbose());
  m_isGui = options.startUI();
  m_isShell = options.startShell();
  m_legacy = new LegacyModules(isGui() ? REQUIRE_INTERFACE: 0);
  m_files = options.files();

  if (options.hasExporterParams()) {
    m_exporter.reset(new DocumentExporter);

    m_exporter->setDataFilename(options.data());
    m_exporter->setTextureFilename(options.sheet());
    m_exporter->setScale(options.scale());
  }

  // Register well-known image file types.
  FileFormatsManager::instance()->registerAllFormats();

  // init editor cursor
  Editor::editor_cursor_init();

  // Load RenderEngine configuration
  RenderEngine::loadConfig();
  if (isPortable())
    PRINTF("Running in portable mode\n");

  // Default palette.
  std::string palFile(!options.paletteFileName().empty() ?
    options.paletteFileName():
    std::string(get_config_string("GfxMode", "Palette", "")));

  if (palFile.empty()) {
    // Try to use a default pixel art palette.
    ResourceFinder rf;
    rf.includeDataDir("palettes/db32.gpl");
    if (rf.findFirst())
      palFile = rf.filename();
  }

  if (!palFile.empty()) {
    PRINTF("Loading custom palette file: %s\n", palFile.c_str());

    base::UniquePtr<Palette> pal(load_palette(palFile.c_str()));
    if (pal.get() != NULL) {
      set_default_palette(pal.get());
    }
    else {
      PRINTF("Error loading custom palette file\n");
    }
  }

  // Set system palette to the default one.
  set_current_palette(NULL, true);
}
コード例 #18
0
ファイル: touchDetectorModule.cpp プロジェクト: pecfw/wysiwyd
void TouchDetectorModule::initializeParameters(ResourceFinder &rf)
{
    moduleName = rf.check("name", Value("touchDetector"), "Module name (string)").asString();
    setName(moduleName.c_str());
    period = rf.check("period", Value(1000 / 30), "Thread period (string)").asInt();
    threshold = rf.check("threshold", Value(50), "Activation threshold (int)").asInt();
    rf.setDefault("clustersConfFile", Value("clustersConfig.ini"));
    clustersConfFilepath = rf.findFile("clustersConfFile");
    
    // get the name of the input and output ports, automatically prefixing the module name by using getName()
    torsoPortName = "/";
    torsoPortName += getName(rf.check("torsoPort", Value("/torso:i"), "Torso input port (string)").asString());
    leftArmPortName = "/";
    leftArmPortName += getName(rf.check("leftArmPort", Value("/left_arm:i"), "Left arm input port (string)").asString());
    rightArmPortName = "/";
    rightArmPortName += getName(rf.check("rightArmPort", Value("/right_arm:i"), "Right arm input port (string)").asString());
    leftForearmPortName = "/";
    leftForearmPortName += getName(rf.check("leftForearmPort", Value("/left_forearm:i"), "Left forearm input port (string)").asString());
    rightForearmPortName = "/";
    rightForearmPortName += getName(rf.check("rightForearmPort", Value("/right_forearm:i"), "Right forearm input port (string)").asString());
    leftHandPortName = "/";
    leftHandPortName += getName(rf.check("leftHandPort", Value("/left_hand:i"), "Left hand input port (string)").asString());
    rightHandPortName = "/";
    rightHandPortName += getName(rf.check("rightHandPort", Value("/right_hand:i"), "Right hand input port (string)").asString());
    touchPortName = "/";
    touchPortName += getName(rf.check("touchPort", Value("/touch:o"), "Touch output port (string)").asString());
}
コード例 #19
0
void partMover::calib_click(GtkButton *button, gtkClassData* currentClassData)
{
  //ask for confirmation
  if (!dialog_question("Do you really want to recalibrate the joint?")) 
  {
     return;
  }

  partMover *currentPart = currentClassData->partPointer;
  int * joint = currentClassData->indexPointer;
  IPositionControl *ipos = currentPart->pos;
  IEncoders *iiencs = currentPart->iencs;
  IAmplifierControl *iamp = currentPart->amp;
  IPidControl *ipid = currentPart->pid;
  IControlCalibration2 *ical = currentPart->cal;
  int NUMBER_OF_JOINTS;
  ipos->getAxes(&NUMBER_OF_JOINTS);

  ResourceFinder *fnd = currentPart->finder;
  //fprintf(stderr, "opening file \n");
  char buffer1[800];
  char buffer2[800];

  strcpy(buffer1, currentPart->partLabel);
  strcpy(buffer2, strcat(buffer1, "_calib"));

  if (!fnd->findGroup(buffer2).isNull())
    {
      bool ok = true;
      Bottle xtmp;

      xtmp.clear();
      xtmp = fnd->findGroup(buffer2).findGroup("CalibrationType");
      ok = ok && (xtmp.size() == NUMBER_OF_JOINTS+1);
      unsigned char type = (unsigned char) xtmp.get(*joint+1).asDouble();
      fprintf(stderr, "%d ", type);

      xtmp.clear();
      xtmp = fnd->findGroup(buffer2).findGroup("Calibration1");
      ok = ok && (xtmp.size() == NUMBER_OF_JOINTS+1);
      double param1 = xtmp.get(*joint+1).asDouble();
      fprintf(stderr, "%f ", param1);

      xtmp.clear();
      xtmp = fnd->findGroup(buffer2).findGroup("Calibration2");
      ok = ok && (xtmp.size() == NUMBER_OF_JOINTS+1);
      double param2 = xtmp.get(*joint+1).asDouble();
      fprintf(stderr, "%f ", param2);

      xtmp.clear();
      xtmp = fnd->findGroup(buffer2).findGroup("Calibration3");
      ok = ok && (xtmp.size() == NUMBER_OF_JOINTS+1);
      double param3 = xtmp.get(*joint+1).asDouble();
      fprintf(stderr, "%f \n", param3);


      if(!ok)
    dialog_message(GTK_MESSAGE_ERROR,(char *)"Check number of calib entries in the group",  buffer2, true);
      else
    ical->calibrate2(*joint, type, param1, param2, param3);
    }
  else
    dialog_message(GTK_MESSAGE_ERROR,(char *)"The supplied file does not conatain a group named:",  buffer2, true);

  return;
}
コード例 #20
0
    //********************************************
    bool configure(ResourceFinder &rf)
    {
        if (rf.check("noLeftArm") && rf.check("noRightArm"))
        {
            yError("[demoAvoidance] no arm has been selected. Closing.");
            return false;
        }

        useLeftArm  = false;
        useRightArm = false;

        string  name=rf.check("name",Value("avoidance")).asString().c_str();
        string robot=rf.check("robot",Value("icub")).asString().c_str();
        motionGain = -1.0;
        if (rf.check("catching"))
        {
            motionGain = 1.0;
        }
        if (motionGain!=-1.0)
        {
            yWarning("[demoAvoidance] motionGain set to catching");
        }

        bool autoConnect=rf.check("autoConnect");
        if (autoConnect)
        {
            yWarning("[demoAvoidance] Autoconnect mode set to ON");
        }

        bool stiff=rf.check("stiff");
        if (stiff)
        {
            yInfo("[demoAvoidance] Stiff Mode enabled.");
        }

        Matrix R(4,4);
        R(0,0)=-1.0; R(2,1)=-1.0; R(1,2)=-1.0; R(3,3)=1.0;

        if (!rf.check("noLeftArm"))
        {
            useLeftArm=true;

            data["left"]=Data();
            data["left"].home_x[0]=-0.30;
            data["left"].home_x[1]=-0.20;
            data["left"].home_x[2]=+0.05;
            data["left"].home_o=dcm2axis(R);

            Property optionCartL;
            optionCartL.put("device","cartesiancontrollerclient");
            optionCartL.put("remote","/"+robot+"/cartesianController/left_arm");
            optionCartL.put("local",("/"+name+"/cart/left_arm").c_str());
            if (!driverCartL.open(optionCartL))
            {
                close();
                return false;
            }

            Property optionJointL;
            optionJointL.put("device","remote_controlboard");
            optionJointL.put("remote","/"+robot+"/left_arm");
            optionJointL.put("local",("/"+name+"/joint/left_arm").c_str());
            if (!driverJointL.open(optionJointL))
            {
                close();
                return false;
            }

            driverCartL.view(data["left"].iarm);
            data["left"].iarm->storeContext(&contextL);

            Vector dof;
            data["left"].iarm->getDOF(dof);
            dof=0.0; dof[3]=dof[4]=dof[5]=dof[6]=1.0;
            data["left"].iarm->setDOF(dof,dof);
            data["left"].iarm->setTrajTime(0.9);

            data["left"].iarm->goToPoseSync(data["left"].home_x,data["left"].home_o);
            data["left"].iarm->waitMotionDone();

            IInteractionMode  *imode; driverJointL.view(imode);
            IImpedanceControl *iimp;  driverJointL.view(iimp);

            if (!stiff)
            {
                imode->setInteractionMode(0,VOCAB_IM_COMPLIANT); iimp->setImpedance(0,0.4,0.03);
                imode->setInteractionMode(1,VOCAB_IM_COMPLIANT); iimp->setImpedance(1,0.4,0.03);
                imode->setInteractionMode(2,VOCAB_IM_COMPLIANT); iimp->setImpedance(2,0.4,0.03);
                imode->setInteractionMode(3,VOCAB_IM_COMPLIANT); iimp->setImpedance(3,0.2,0.01);
                imode->setInteractionMode(4,VOCAB_IM_COMPLIANT); iimp->setImpedance(4,0.2,0.0);
            }
        }

        if (!rf.check("noRightArm"))
        {
            useRightArm = true;

            data["right"]=Data();
            data["right"].home_x[0]=-0.30;
            data["right"].home_x[1]=+0.20;
            data["right"].home_x[2]=+0.05;
            data["right"].home_o=dcm2axis(R);

            Property optionCartR;
            optionCartR.put("device","cartesiancontrollerclient");
            optionCartR.put("remote","/"+robot+"/cartesianController/right_arm");
            optionCartR.put("local",("/"+name+"/cart/right_arm").c_str());
            if (!driverCartR.open(optionCartR))
            {
                close();
                return false;
            }

            Property optionJointR;
            optionJointR.put("device","remote_controlboard");
            optionJointR.put("remote","/"+robot+"/right_arm");
            optionJointR.put("local",("/"+name+"/joint/right_arm").c_str());
            if (!driverJointR.open(optionJointR))
            {
                close();
                return false;
            }

            driverCartR.view(data["right"].iarm);
            data["right"].iarm->storeContext(&contextR);

            Vector dof;
            data["right"].iarm->getDOF(dof);
            dof=0.0; dof[3]=dof[4]=dof[5]=dof[6]=1.0;
            data["right"].iarm->setDOF(dof,dof);
            data["right"].iarm->setTrajTime(0.9);

            data["right"].iarm->goToPoseSync(data["right"].home_x,data["right"].home_o);
            data["right"].iarm->waitMotionDone();


            IInteractionMode  *imode; driverJointR.view(imode);
            IImpedanceControl *iimp;  driverJointR.view(iimp);

            if (!stiff)
            {
                imode->setInteractionMode(0,VOCAB_IM_COMPLIANT); iimp->setImpedance(0,0.4,0.03);
                imode->setInteractionMode(1,VOCAB_IM_COMPLIANT); iimp->setImpedance(1,0.4,0.03);
                imode->setInteractionMode(2,VOCAB_IM_COMPLIANT); iimp->setImpedance(2,0.4,0.03);
                imode->setInteractionMode(3,VOCAB_IM_COMPLIANT); iimp->setImpedance(3,0.2,0.01);
                imode->setInteractionMode(4,VOCAB_IM_COMPLIANT); iimp->setImpedance(4,0.2,0.0);
            }
        }

        dataPort.open(("/"+name+"/data:i").c_str());
        dataPort.setReader(*this);

        if (autoConnect)
        {
            Network::connect("/visuoTactileRF/skin_events:o",("/"+name+"/data:i").c_str());
        }

        rpcSrvr.open(("/"+name+"/rpc:i").c_str());
        attach(rpcSrvr);
        return true;
    }
コード例 #21
0
ファイル: main.cpp プロジェクト: fhaust/event-driven
    virtual bool configure(ResourceFinder &rf)
    {
        Time::turboBoost();

        fprintf(stderr, "Getting projections\n");

        Matrix PiRight;
        Bottle b;
        b = rf.findGroup("CAMERA_CALIBRATION_RIGHT");
        //fprintf(stderr, "CAMERA_CALIBRATION_RIGHT contains: %s\n", b.toString().c_str());
        if (getProjectionMatrix(b, PiRight) == 0)
          {
            fprintf(stderr, "CAMERA_CALIBRATION_RIGHT was missing some params\n");
            return false;
          }
        else
          {
            fprintf(stderr, "Working with RightProjection \n");
            for (int i=0; i < PiRight.rows(); i++)
              fprintf(stderr, "%s\n", PiRight.getRow(i).toString().c_str());
          }

        Matrix PiLeft;
        b = rf.findGroup("CAMERA_CALIBRATION_LEFT");
        //fprintf(stderr, "CAMERA_CALIBRATION_LEFT contains: %s\n", b.toString().c_str());
        if (getProjectionMatrix(b, PiLeft) == 0)
          {
            fprintf(stderr, "CAMERA_CALIBRATION_LEFT was missing some params\n");
            return false;
          }
        else
          {
            fprintf(stderr, "Working with LeftProjection \n");
            for (int i=0; i < PiLeft.rows(); i++)
              fprintf(stderr, "%s\n", PiLeft.getRow(i).toString().c_str());
          }

        int period=50;
        if (rf.check("period"))
            period=rf.find("period").asInt();

        bool enableKalman=false;
        if (rf.check("kalman"))
            enableKalman=true;

        string ctrlName=rf.find("name").asString().c_str();
        string robotName=rf.find("robot").asString().c_str();

        fprintf(stderr, "Initializing eT\n");
        eT=new eyeTriangulation(rf, PiLeft, PiRight, enableKalman, period, ctrlName, robotName);

        Vector xr(3); xr(0)=PiRight(0,2); xr(1)=PiRight(1,2); xr(2)=1.0; 
        Vector xl(3); xl(0)=PiLeft(0,2);  xl(1)=PiLeft(1,2);  xl(2)=1.0; 
        eT->xInit(xl, xr);

        if (rf.check("const"))
            eT->xSetConstant(xl, xr);

        eT->start();    

        string rpcPortName="/"+ctrlName+"/rpc";
        rpcPort.open(rpcPortName.c_str());
        attach(rpcPort);

        return true;
    }
コード例 #22
0
ファイル: main.cpp プロジェクト: AbuMussabRaja/icub-main
int main(int argc, char *argv[]) 
{
    Network yarp; //initialize network, this goes before everything

    if (!yarp.checkNetwork())
    {
        fprintf(stderr, "Sorry YARP network does not seem to be available, is the yarp server available?\n");
        return -1;
    }

    yarp::os::signal(yarp::os::YARP_SIGINT, sighandler);
    yarp::os::signal(yarp::os::YARP_SIGTERM, sighandler);

    Time::turboBoost();  

    //for compatibility with old usage of iCubInterface, the use of the ResourceFinder
    //here is merely functional and should NOT be taken as an example
    ResourceFinder rf;
    rf.setVerbose();
    rf.setDefaultConfigFile("iCubInterface.ini");
    rf.configure(argc, argv);
    ConstString configFile=rf.findFile("config");
    ConstString cartRightArm=rf.findFile("cartRightArm");
    ConstString cartLeftArm=rf.findFile("cartLeftArm");

    std::string filename;
    bool remap=false;
    if (configFile!="")
    {
        configFile=rf.findFile("config");
        filename=configFile.c_str();
        remap=true;
    }
    else
    {
        configFile=rf.find("file").asString();
        if (configFile!="")
        {
            const char *conf = yarp::os::getenv("ICUB_ROOT");
            filename+=conf;
            filename+="/conf/";
            filename+=configFile.c_str();
            printf("Read robot description from %s\n", filename.c_str());
        }
        else
        {
            printf("\n");
            printf("Error: iCubInterface was not able to find a valid configuration file ");
            printf("(since September 2009 we changed a bit how iCubInterface locates configuratrion files).\n");
            printf("== Old possibilities:\n");
            printf("--config <CONFIG_FILE> read config file CONFIG_FILE.\n iCubInterface now ");
            printf("uses the ResourceFinder class to search for CONFIG_FILE. ");
            printf("Make sure you understand how the ResourceFinder works. In particular ");
            printf("you most likely need to set the YARP_ROBOT_NAME environment variable ");
            printf("to tell the RF to add robots/$YARP_ROBOT_NAME to the search path.\n");
            printf("--file <CONFIG_FILE> read config file from $ICUB_ROOT/conf: old style ");
            printf("initialization method, obsolete. Still here for compatibility reasons.\n");
            printf("== New possibilities:\n");
            printf("Place a file called iCubInterface.ini in robots/$YARP_ROBOT_NAME that contains ");
            printf("the line \"config icubSafe.ini\" (or anything of your choice), and run iCubInterface ");
            printf("without parameters\n.");
            printf("== Preventing default behaviors:\n");
            printf("Use full path in <CONFIG_FILE> (e.g. --config ./icubSafe.ini).\n");
            printf("Use --from: change config file (e.g. --from iCubInterfaceCustom.ini).\n");
            return -1;
        }
    }
    //      printf("--file <CONFIG_FILE>  read robot config file CONFIG_FILE\n");
    //     printf("Note: this files are searched in $ICUB_ROOT/conf (obsolete)\n");
    //     printf("--config <CONFIG_FILE> full path to robot config file\n");
    //    printf("-Examples:\n");
    //    printf("%s --file icub.ini (obsolete, not standard)\n", argv[0]);
    //     printf("%s --config $ICUB_ROOT/app/default/conf/icub.ini\n)", argv[0]);
    //    return -1;


    Property robotOptions;
    bool ok=robotOptions.fromConfigFile(filename.c_str());
    if (!ok) 
    {
        fprintf(stderr, "Sorry could not open %s\n", filename.c_str());
        return -1;
    }

    printf("Config file loaded successfully\n");

    std::string quitPortName;
    if (robotOptions.check("TERMINATEPORT"))
    {
        Value &v=robotOptions.findGroup("TERMINATEPORT").find("Name");
        quitPortName=std::string(v.toString().c_str());
    }
    else
        quitPortName=std::string("/iCubInterface/quit");

    fprintf(stderr, "Quit will listen on %s\n", quitPortName.c_str());

    // LATER: must use a specific robot name to make the port unique.
    Terminee terminee(quitPortName.c_str());

    //    Terminee terminee("/iCubInterface/quit");
    if (!terminee.isOk()) { 
        printf("Failed to create proper quit socket\n"); 
        return 1;
    }   

    IRobotInterface *i;
    if (remap)
    {
        i=new RobotInterfaceRemap;
    }
    else
    {
        i=new RobotInterface;
    }
    ri=i; //set pointer to RobotInterface object (used in the handlers above)

    ok = i->initialize(filename); 
    if (!ok)
        return 0;

    char c = 0;    
    printf("Driver instantiated and running (silently)\n");

    printf("Checking if you have requested instantiation of cartesian controller\n");
    bool someCartesian=false;
    if (cartRightArm!="")
    {
        i->initCart(cartRightArm.c_str());
        someCartesian=true;
    }
    if (cartLeftArm!="")
    {
        i->initCart(cartLeftArm.c_str());
        someCartesian=true;
    }
    if (!someCartesian)
    {
        printf("Nothing found\n");
    }

    while (!terminee.mustQuit()&&!terminated) 
    {
        Time::delay(2); 
    }

    printf("Received a quit message\n");

    if (someCartesian)
    {
        i->finiCart();
    }

    i->detachWrappers();
    i->park(); //default behavior is blocking (safer)
    i->closeNetworks();
    ri=0;  //tell signal handler interface is not longer valid (do this before you destroy i ;)
    delete i; 
    i=0;
    return 0;  
}
コード例 #23
0
ファイル: main.cpp プロジェクト: apaikan/icub-main
    bool configure(ResourceFinder &rf)
    {
        bool noLegs=rf.check("no_legs");

        Property optTorso("(device remote_controlboard)");
        Property optArmR("(device remote_controlboard)");
        Property optArmL("(device remote_controlboard)");
        Property optLegR("(device remote_controlboard)");
        Property optLegL("(device remote_controlboard)");

        string robot=rf.find("robot").asString().c_str();
        optTorso.put("remote",("/"+robot+"/torso").c_str());
        optArmR.put("remote",("/"+robot+"/right_arm").c_str());
        optArmL.put("remote",("/"+robot+"/left_arm").c_str());
        optLegR.put("remote",("/"+robot+"/right_leg").c_str());
        optLegL.put("remote",("/"+robot+"/left_leg").c_str());

        string local=rf.find("local").asString().c_str();
        optTorso.put("local",("/"+local+"/torso").c_str());
        optArmR.put("local",("/"+local+"/right_arm").c_str());
        optArmL.put("local",("/"+local+"/left_arm").c_str());
        optLegR.put("local",("/"+local+"/right_leg").c_str());
        optLegL.put("local",("/"+local+"/left_leg").c_str());

        optTorso.put("writeStrict","on");
        optArmR.put("writeStrict","on");
        optArmL.put("writeStrict","on");
        optLegR.put("writeStrict","on");
        optLegL.put("writeStrict","on");

        if (!torso.open(optTorso) || !armR.open(optArmR) || !armL.open(optArmL))
        {
            cout<<"Device drivers not available!"<<endl;
            close();

            return false;
        }

        if (!noLegs)
        {
            if (!legR.open(optLegR) || !legL.open(optLegL))
            {
                cout<<"Device drivers not available!"<<endl;
                close();

                return false;
            }
        }

        PolyDriverList listArmR, listArmL, listLegR, listLegL;
        listArmR.push(&torso,"torso");
        listArmR.push(&armR,"right_arm");
        listArmL.push(&torso,"torso");
        listArmL.push(&armL,"left_arm");
        listLegR.push(&legR,"right_leg");
        listLegL.push(&legL,"left_leg");

        Property optServerArmR("(device cartesiancontrollerserver)");
        Property optServerArmL("(device cartesiancontrollerserver)");
        Property optServerLegR("(device cartesiancontrollerserver)");
        Property optServerLegL("(device cartesiancontrollerserver)");
        optServerArmR.fromConfigFile(rf.findFile("right_arm_file"),false);
        optServerArmL.fromConfigFile(rf.findFile("left_arm_file"),false);
        optServerLegR.fromConfigFile(rf.findFile("right_leg_file"),false);
        optServerLegL.fromConfigFile(rf.findFile("left_leg_file"),false);

        if (!serverArmR.open(optServerArmR) || !serverArmL.open(optServerArmL))
        {
            close();    
            return false;
        }

        if (!noLegs)
        {
            if (!serverLegR.open(optServerLegR) || !serverLegL.open(optServerLegL))
            {
                close();    
                return false;
            }
        }

        IMultipleWrapper *wrapperArmR, *wrapperArmL, *wrapperLegR, *wrapperLegL;
        serverArmR.view(wrapperArmR);
        serverArmL.view(wrapperArmL);
        serverLegR.view(wrapperLegR);
        serverLegL.view(wrapperLegL);

        if (!wrapperArmR->attachAll(listArmR) || !wrapperArmL->attachAll(listArmL))
        {
            close();    
            return false;
        }

        if (!noLegs)
        {
            if (!wrapperLegR->attachAll(listLegR) || !wrapperLegL->attachAll(listLegL))
            {
                close();    
                return false;
            }
        }

        return true;
    }
コード例 #24
0
ファイル: main.cpp プロジェクト: Karma-Revolutions/yarp
int main(int argc, char *argv[])
{
    QApplication a(argc, argv);

    //YARP_REGISTER_DEVICES(icubmod)

    Network yarp;

    if (!yarp.checkNetwork())
    {
        LOG_ERROR("Error initializing yarp network (is yarpserver running?)\n");
        QMessageBox::critical(0,"Error","Error initializing yarp network (is yarpserver running?)");
        return 0;
    }

    ResourceFinder *finder;

    Property p, q;
    finder = new ResourceFinder;

    //retrieve information for the list of parts
    finder->setVerbose();
    finder->setDefaultConfigFile("yarpmotorgui.ini");
    finder->setDefault("name", "icub");
    finder->configure(argc,argv);

    qRegisterMetaType<Pid>("Pid");
    qRegisterMetaType<SequenceItem>("SequenceItem");
    qRegisterMetaType<QList<SequenceItem> >("QList<SequenceItem>");


    if (finder->check("calib")){
        LOG("Calibrate buttons on\n");
        enable_calib_all = true;
    }
    if (finder->check("admin")){
        LOG("Admin mode on.\n");
        enable_calib_all = true;
        debug_param_enabled = false;
        position_direct_enabled = true;
        openloop_enabled = true;
        old_impedance_enabled = true;
    }
    if (finder->check("debug")){
        LOG("Debug interface requested.\n");
        debug_param_enabled = false;
    }
    if (finder->check("speed")){
        LOG("Speed view requested.\n");
        speedview_param_enabled = true;
    }
    if (finder->check("direct")){
        LOG("Position direct requested.\n");
        position_direct_enabled = true;
    }
    if (finder->check("openloop")){
        LOG("Openloop requested.\n");
        openloop_enabled = true;
    }

    bool deleteParts=false;
    std::string robotName=finder->find("name").asString().c_str();
    yDebug("Robot name: %s\n",robotName.data());

    Bottle *pParts=finder->find("parts").asList();
    if (pParts==NULL){
        printf("Setting default parts.\n");
        pParts=new Bottle("head torso left_arm right_arm left_leg right_leg");
        deleteParts=true;
    }

    NUMBER_OF_AVAILABLE_PARTS=pParts->size();
    if (NUMBER_OF_AVAILABLE_PARTS > MAX_NUMBER_ACTIVATED){
        LOG_ERROR("The number of parts exceeds the maximum! \n");
        return 0;
    }
    if (NUMBER_OF_AVAILABLE_PARTS<=0){
        LOG_ERROR("Invalid number of parts, check config file \n");
        return 0;
    }

    for(int n=0; n < MAX_NUMBER_ACTIVATED; n++){
        //ENA = 0: part available
        //ENA = -1: part unavailable
        //ENA = 1: part used
        ENA.append(-1);
    }

    //Check 1 in the panel
    for(int n=0;n<NUMBER_OF_AVAILABLE_PARTS;n++){
        QString part = QString("%1").arg(pParts->get(n).asString().c_str());
        yDebug("Appending %s",part.toUtf8().constData());
        partsName.append(part);

        if(n < ENA.count()){
            ENA.replace(n,1);
        }else{
            ENA.append(1);
        }
    }

    QString newRobotName = robotName.data();

    if (!finder->check("skip"))
    {
        StartDlg dlg;
        dlg.init(QString(robotName.data()),partsName,ENA);
        if(dlg.exec() == QDialog::Accepted){
            ENA.clear();
            ENA = dlg.getEnabledParts();
            newRobotName = dlg.getRobotName();
        }else{
            yInfo("Cancel Button pressed. Closing..");
            return 0;
        }
    }

    QStringList enabledParts;
    for(int i=0; i<partsName.count();i++){
        if(ENA.at(i) == 1){
            enabledParts.append(partsName.at(i));
        }
    }

    yarp::os::signal(yarp::os::YARP_SIGINT, sighandler);
    yarp::os::signal(yarp::os::YARP_SIGTERM, sighandler);
    MainWindow w;
    mainW = &w;
    int appRet = 0;
    bool ret = w.init(newRobotName,enabledParts,finder,debug_param_enabled,speedview_param_enabled,enable_calib_all,position_direct_enabled,openloop_enabled);
    if(ret){
        w.show();
        appRet = a.exec();
    }

    delete finder;
    if(deleteParts){
        delete pParts;
    }

    return appRet;
}
コード例 #25
0
bool TactileLearning::getFingersAndJoints(ResourceFinder& rf)
{
	/**
	* There are two mutual exclusive ways to specify which fingers and joints are involved in the problem.
	* 1. Checking which fingers are active and setting the joints properly.
	* 2. Setting by hand the joint vector.
	* In both cases checks for consistency have to be done, which higher priority to the fingers vector. 
	*/

	string tmp;
	int jointIndex;

	if(!rf.findGroup("FINGERS").isNull())
	{
		// check which fingers are specified in the configuration file and set the activeFingers vector accordingly
		for(int i = 0; i < NUM_FINGERS; i++)
		{
			tmp = rf.findGroup("FINGERS").find(FINGER_TYPES[i].c_str()).asString();
			if(tmp.compare("on") == 0) activeFingers[i] = true;
			else activeFingers[i] = false;
		}

		/**
		* Set the joints vector so that it is consistent with the fingers vector values. This step requires 
		* knowledge about hardware implementation of the robot hand.
		*/
		jointIndex = 0;

		//------------------------------------------------------------------------------------------------------------
		// WARNING! The joint of the thumb to be ignored is the 9-th in the simulator, but the 8-th in the real robot!
		//------------------------------------------------------------------------------------------------------------
		// thumb
		if(activeFingers[0] == true)
		{
			for(int i = 0; i < 3; i++)
			{
				// for simulator: ignore second joint (9-th for the whole arm)
				// for real robot: ignore first joint (8-th for the whole arm)
				if(i != 0) activeJoints[jointIndex++] = true;
				else activeJoints[jointIndex++] = false;
			}
		}
		else
		{
			for(int i = 0; i < 3; i++) activeJoints[jointIndex++] = false;
		}

		// index
		if(activeFingers[1] == true)
		{
			for(int i = 0; i < 2; i++) activeJoints[jointIndex++] = true;
		}
		else
		{
			for(int i = 0; i < 2; i++) activeJoints[jointIndex++] = false;
		}

		// middle
		if(activeFingers[2] == true)
		{
			for(int i = 0; i < 2; i++) activeJoints[jointIndex++] = true;
		}
		else
		{
			for(int i = 0; i < 2; i++) activeJoints[jointIndex++] = false;
		}

		// ring + pinky
		if(activeFingers[3] == true) activeJoints[jointIndex] = true;
		else activeJoints[jointIndex] = false;

	}

	else if(rf.check("joints"))
	{
		Bottle joints = rf.findGroup("joints").tail();

		// check which joints are specified in the configuration file and set the activeJoints vector accordingly.
		// brute force but who cares
		for(int i = 0; i < NUM_HAND_JOINTS; i++)
		{
			activeJoints[i] = false;
			for(int j = 0; j < joints.size(); j++)
			{
				if(i == joints.get(j).asInt() - HAND_JOINTS_OFFSET)
				{
					activeJoints[i] = true;
					break;
				}
			}
		}

		/**
		* activate each finger if at least a joint related to it has been activated.
		* This step requires knowledge about hardware implementation of the robot hand.
		*/

		// thumb
		if(activeJoints[0] || activeJoints[1] || activeJoints[2]) activeFingers[0] = true;
		else activeFingers[0] = false;
		
		// index
		if(activeJoints[3] || activeJoints[4]) activeFingers[1] = true;
		else activeFingers[1] = false;

		// middle
		if(activeJoints[5] || activeJoints[6]) activeFingers[2] = true;
		else activeFingers[2] = false;

		// pinky
		if(activeJoints[7]) activeFingers[3] = true;
		else activeFingers[3] = false;
	}
	else
	{
		printf("ERROR: no fingers were set, please check configuration file.\n");
		return false;
	}

	// check for at least one active finger and in this case print both active fingers and active joints
	for(int i = 0; i < NUM_FINGERS; i++)
	{
		if(activeFingers[i])
		{
			printFingersAndJoints();
			return true;
		}
	}

	// if no fingers are active the initialization is not successful
	printf("ERROR: no fingers were set, please check configuration file.\n");
	return false;
}
コード例 #26
0
ファイル: rt2objCol.cpp プロジェクト: robotology/wysiwyd
Reactable2OPC::Reactable2OPC()
{
    H2ICUB = eye(4);
    H2RT = eye(4);
    isCalibrated = false;
    portCalibration.open("/reactable2opc/calibration:rpc");

    w = new OPCClient("reactable2opc");
    w->isVerbose = false;

    while ( !w->connect("OPC") || !Network::connect("/reactable2opc/calibration:rpc","/referenceFrameHandler/rpc"))
    {
        cout<<"Waiting for connection to OPC or rfh..."<<endl;
        Time::delay(1.0);
    }
    //

    //Load the id/name mapping from a config file
    ResourceFinder rf;
    rf.setVerbose(false);
    rf.setDefaultContext("reactable2opc");
    rf.setDefaultConfigFile("mapping.ini");
    rf.configure(0,NULL);
    bool invertYaxis = rf.check("invertYaxis",Value(0)).asInt() == 1;
    bool invertXaxis = rf.check("invertXaxis",Value(0)).asInt() == 1;
    if (invertYaxis)
    {
        cout<<"Using inverted Y axis"<<endl;
        YaxisFactor = -1;
    }
    else
        YaxisFactor = 1;

    if (invertXaxis)
    {
        cout<<"Using inverted X axis"<<endl;
        XaxisFactor = -1;
    }
    else
        XaxisFactor = 1;

    loadObjectsDatabase(rf);

    //Open the OSC socket for reception
    OSC_INPUT_TABLE_PORT = rf.check("OSC_TABLE_INPUT_PORT", Value(3333)).asInt();
    int oscReceiverPort = rf.check("OSC_INPUT_PORT",Value(3334)).asInt();
    oscThreadReceiver = new OscThread(w, oscReceiverPort);
    oscThreadReceiver->start();

    //Open the OSC socket for writing
    int oscEmitterPort = rf.check("OSC_OUTPUT_PORT",Value(3335)).asInt();
    string oscEmitterEndPoint = rf.check("OSC_OUTPUT_ADRESS",Value("10.0.0.106")).asString().c_str();
    oscEmitter = new UdpTransmitSocket(IpEndpointName( oscEmitterEndPoint.c_str(), oscEmitterPort ));
    yarp2osc = new DataPort(oscEmitter);
    yarp2osc->useCallback();
    yarp2osc->open("/reactable2opc/command:i");
    cout<<"Targeting OSC to" <<oscEmitterEndPoint<<":"<<oscEmitterPort<< endl;
    cout<<"OSC & TUIO listening..." << endl;

    checkCalibration();
}
コード例 #27
0
ファイル: main.cpp プロジェクト: xufango/contrib_bk
    MainWindow(ResourceFinder *rf)
    {

        //if (VERBOSE) fprintf(stderr, "Passing RF by address ...");
        resFind = rf;
        //if (VERBOSE) fprintf(stderr, "done \n");

        Property options;
        options.fromString(resFind->toString());

        Value &robot  = options.find("robot");
        Value &part   = options.find("part");
        Value &rows   = options.find("rows");
        Value &cols   = options.find("cols");

        if (!options.check("robot"))
            printf("Missing --robot option. Using icub.\n");
        if (!options.check("part"))
            printf("Missing --part option. Using head.\n");    
        if (!options.check("local"))
            printf("Missing --name option. Using /portScope/vector:i.\n");
        if (!options.check("remote"))
            printf("Missing --remote option. Will wait for the connection...\n");
        if (!options.check("rows"))
            printf("Missing --rows option. Disabling subplotting.\n");

        //if (VERBOSE) fprintf(stderr, "Start plotting the GUI\n");
        QToolBar *toolBar = new QToolBar(this);
        toolBar->setFixedHeight(80);
    
#if QT_VERSION < 0x040000
        setDockEnabled(TornOff, true);
        setRightJustification(true);
#else
        toolBar->setAllowedAreas(Qt::TopToolBarArea | Qt::BottomToolBarArea);
#endif
        QWidget *hBox = new QWidget(toolBar);
        QLabel *label = new QLabel("Timer Interval", hBox);
        QwtCounter *counter = new QwtCounter(hBox);

        QLabel *labelActions = new QLabel("Actions", hBox);
        QPushButton *toggleAcquireButton = new QPushButton("stop", hBox, "stop");
        counter->setRange(-1.0, 1000.0, 1.0);
    
        QHBoxLayout *layout = new QHBoxLayout(hBox);
        layout->addWidget(label);
        layout->addWidget(counter);
        layout->addWidget(labelActions);
        layout->addWidget(toggleAcquireButton);
        layout->addWidget(new QWidget(hBox), 10); // spacer);
    
#if QT_VERSION >= 0x040000
        toolBar->addWidget(hBox);
#endif
        addToolBar(toolBar);
    
        if (options.check("rows"))
            nRows = rows.asInt();
        else
            nRows = 1;

        if (options.check("cols"))
            nCols = cols.asInt();
        else
            nCols = nRows;

        nPlots  = nRows*nCols;
        nPortInputsInPlots = new int[nPlots];
        QGrid *grid = new QGrid( nRows, Qt::Vertical,  this );
        
        plot = new DataPlot*[nPlots];
        setCentralWidget(grid);

        for( int r = 0 ; r < nRows ; r++ )
            {
                for( int c = 0 ; c < nCols ; c++ )
                    {	  
                        char rcS[256];
                        sprintf(rcS, "%d%d", r, c);
                        ConstString rS(rcS);
                        ConstString local;
                        local = resFind->find("local").toString();
                        //local port name
                        local = local + rcS;

                        //dataPlot creation
                        plot[r+nRows*c] = new DataPlot(grid);
	    
                        //dataPlot set plot to read from
                        setInputPort(resFind, r+nRows*c, local);

                        //dataPlot set indeces of ports to plot
                        setIndexMask(resFind, r+nRows*c, nPortInputsInPlots[r+nRows*c]);

                    }
            }
	   
        for( int r = 0 ; r < nRows ; r++ )
            {
                for( int c = 0 ; c < nCols ; c++ )
                    { 
                        //stop acquisition button
                        connect( toggleAcquireButton, SIGNAL(clicked()), plot[r+nRows*c], SLOT(toggleAcquire()) );
                        plot[r+nRows*c]->initSignalDimensions();
                        //counter button
                        connect(counter, SIGNAL(valueChanged(double)),
                                plot[r+nRows*c], SLOT(setTimerInterval(double)) );
	  
                        counter->setValue(50.0);
	 
                        QSizePolicy sizePolicy2(QSizePolicy::Maximum, QSizePolicy::Maximum);
                        grid->setSizePolicy(sizePolicy2);

                        QSizePolicy sizePolicy(QSizePolicy::Maximum, QSizePolicy::Maximum);
                        sizePolicy.setHeightForWidth(plot[r+nRows*c]->sizePolicy().hasHeightForWidth());
                        plot[r+nRows*c]->setSizePolicy(sizePolicy);

                        //QSize gridSize= grid->sizeHint();
                        //gridSize.setWidth(500);
                        //gridSize.setHeight(500);
                        plot[r+nRows*c]->setMinimumWidth(10);
                        plot[r+nRows*c]->setMaximumWidth(800);
                        plot[r+nRows*c]->setMinimumHeight(10);
                        plot[r+nRows*c]->setMaximumHeight(800);
                        //if (VERBOSE) fprintf(stderr, "Grid max is: hInt=%d, vInt=%d\n", grid->maximumHeight(), grid->maximumWidth()); 
                        //if (VERBOSE) fprintf(stderr, "Plot max is: hInt=%d, vInt=%d\n", plot[r+nRows*c]->maximumHeight(), plot[r+nRows*c]->maximumWidth()); 
                        //QSize plotSize= plot[r+nRows*c]->sizeHint();
                        //if (VERBOSE) fprintf(stderr, "Plot Hint is: hInt=%d, vInt=%d\n", plotSize.height(), plotSize.width()); 
                    }
            }
    }
コード例 #28
0
    bool configure(ResourceFinder &rf)
    {
        name     = "visuoTactileRF";
        robot    = "icub";
        modality = "1D";

        verbosity  = 0;     // verbosity
        rate       = 20;    // rate of the vtRFThread

        //******************************************************
        //********************** CONFIGS ***********************

        //******************* NAME ******************
            if (rf.check("name"))
            {
                name = rf.find("name").asString();
                yInfo("Module name set to %s", name.c_str());
            }
            else yInfo("Module name set to default, i.e. %s", name.c_str());
            setName(name.c_str());

        //******************* ROBOT ******************
            if (rf.check("robot"))
            {
                robot = rf.find("robot").asString();
                yInfo("Robot is %s", robot.c_str());
            }
            else yInfo("Could not find robot option in the config file; using %s as default",robot.c_str());

        //***************** MODALITY *****************
            if (rf.check("modality"))
            {
                modality = rf.find("modality").asString();
                yInfo("modality is  %s", modality.c_str());
            }
            else yInfo("Could not find modality option in the config file; using %s as default",modality.c_str());

        //******************* VERBOSE ******************
            if (rf.check("verbosity"))
            {
                verbosity = rf.find("verbosity").asInt();
                yInfo("vtRFThread verbosity set to %i", verbosity);
            }
            else yInfo("Could not find verbosity option in config file; using %i as default",verbosity);

        //****************** rate ******************
            if (rf.check("rate"))
            {
                rate = rf.find("rate").asInt();
                yInfo("vtRFThread working at  %i ms", rate);
            }
            else yInfo("Could not find rate in the config file; using %i ms as default",rate);

        //************* skinTaxels' Resource finder **************
            ResourceFinder skinRF;
            skinRF.setVerbose(false);
            skinRF.setDefaultContext("skinGui");                //overridden by --context parameter
            skinRF.setDefaultConfigFile("skinManForearms.ini"); //overridden by --from parameter
            skinRF.configure(0,NULL);

            vector<string> filenames;
            int partNum=4;

            Bottle &skinEventsConf = skinRF.findGroup("SKIN_EVENTS");
            if(!skinEventsConf.isNull())
            {
                yInfo("SKIN_EVENTS section found\n");

                if(skinEventsConf.check("skinParts"))
                {
                    Bottle* skinPartList = skinEventsConf.find("skinParts").asList();
                    partNum=skinPartList->size();
                }

                if(skinEventsConf.check("taxelPositionFiles"))
                {
                    Bottle *taxelPosFiles = skinEventsConf.find("taxelPositionFiles").asList();

                    if (rf.check("leftHand") || rf.check("leftForeArm") || rf.check("rightHand") || rf.check("rightForeArm"))
                    {
                        if (rf.check("leftHand"))
                        {
                            string taxelPosFile = taxelPosFiles->get(0).asString().c_str();
                            string filePath(skinRF.findFile(taxelPosFile.c_str()));
                            if (filePath!="")
                            {
                                yInfo("[visuoTactileRF] filePath [%i] %s\n",0,filePath.c_str());
                                filenames.push_back(filePath);
                            }
                        }
                        if (rf.check("leftForeArm"))
                        {
                            string taxelPosFile = taxelPosFiles->get(1).asString().c_str();
                            string filePath(skinRF.findFile(taxelPosFile.c_str()));
                            if (filePath!="")
                            {
                                yInfo("[visuoTactileRF] filePath [%i] %s\n",1,filePath.c_str());
                                filenames.push_back(filePath);
                            }
                        }
                        if (rf.check("rightHand"))
                        {
                            string taxelPosFile = taxelPosFiles->get(2).asString().c_str();
                            string filePath(skinRF.findFile(taxelPosFile.c_str()));
                            if (filePath!="")
                            {
                                yInfo("[visuoTactileRF] filePath [%i] %s\n",2,filePath.c_str());
                                filenames.push_back(filePath);
                            }
                        }
                        if (rf.check("rightForeArm"))
                        {
                            string taxelPosFile = taxelPosFiles->get(3).asString().c_str();
                            string filePath(skinRF.findFile(taxelPosFile.c_str()));
                            if (filePath!="")
                            {
                                yInfo("[visuoTactileRF] filePath [%i] %s\n",3,filePath.c_str());
                                filenames.push_back(filePath);
                            }
                        }
                    }
                    else
                    {
                        for(int i=0;i<partNum;i++)     // all of the skinparts
                        {
                            string taxelPosFile = taxelPosFiles->get(i).asString().c_str();
                            string filePath(skinRF.findFile(taxelPosFile.c_str()));
                            if (filePath!="")
                            {
                                yInfo("[visuoTactileRF] filePath [%i] %s\n",i,filePath.c_str());
                                filenames.push_back(filePath);
                            }
                        }
                    }
                }
            }
            else
            {
                yError(" No skin's configuration files found.");
                return 0;
            }

        //*************** eyes' Resource finder ****************
            ResourceFinder gazeRF;
            gazeRF.setVerbose(verbosity!=0);
            gazeRF.setDefaultContext("iKinGazeCtrl");
            robot=="icub"?gazeRF.setDefaultConfigFile("config.ini"):gazeRF.setDefaultConfigFile("configSim.ini");
            gazeRF.configure(0,NULL);
            double head_version=gazeRF.check("head_version",Value(1.0)).asDouble();

            ResourceFinder eyeAlignRF;

            Bottle &camerasGroup = gazeRF.findGroup("cameras");

            if(!camerasGroup.isNull())
            {
                eyeAlignRF.setVerbose(verbosity!=0);
                camerasGroup.check("context")?
                eyeAlignRF.setDefaultContext(camerasGroup.find("context").asString().c_str()):
                eyeAlignRF.setDefaultContext(gazeRF.getContext().c_str());
                eyeAlignRF.setDefaultConfigFile(camerasGroup.find("file").asString().c_str()); 
                eyeAlignRF.configure(0,NULL); 
            }
            else
            {
                yWarning("Did not find camera calibration group into iKinGazeCtrl ResourceFinder!");        
            }

        //******************************************************
        //*********************** THREAD **********************
            if( filenames.size() > 0 )
            {
                vtRFThrd = new vtRFThread(rate, name, robot, modality, verbosity, rf,
                                          filenames, head_version, eyeAlignRF);
                if (!vtRFThrd -> start())
                {
                    delete vtRFThrd;
                    vtRFThrd = 0;
                    yError("vtRFThread wasn't instantiated!!");
                    return false;
                }
                yInfo("VISUO TACTILE RECEPTIVE FIELDS: vtRFThread instantiated...");
            }
            else {
                vtRFThrd = 0;
                yError("vtRFThread wasn't instantiated. No filenames have been given!");
                return false;
            }

        //******************************************************
        //************************ PORTS ***********************
            rpcClnt.open(("/"+name+"/rpc:o").c_str());
            rpcSrvr.open(("/"+name+"/rpc:i").c_str());
            attach(rpcSrvr);

        return true;
    }
コード例 #29
0
ファイル: main.cpp プロジェクト: xufango/contrib_bk
    void setIndexMask(ResourceFinder *rf, int index, int numberOfInputs)
    {
        int **indexToPlot = NULL;
        indexToPlot = new int*[numberOfInputs];
        int *sizeIndexToPlot = NULL;
        sizeIndexToPlot = new int[numberOfInputs];
        if (rf->find("index").isList())
            {
                Bottle *rrBot;
                //if (VERBOSE) fprintf(stderr, "Option remote is a list\n");
                rrBot = resFind->find("index").asList();
                if (rrBot->size() == nRows*nCols)
                    {
                        if (rrBot->get(index).isInt())
                            {
                                if (numberOfInputs==1)
                                    {
                                        indexToPlot[0] = new int;
                                        indexToPlot[0][0] = rrBot->get(index).asInt();
                                        sizeIndexToPlot[0] = 1;
                                    }
                                else
                                    fprintf(stderr, "ERROR: a single index was supplied but more than one input port was given!\n");
                            }
                        else if (rrBot->get(index).isList())
                            {
                                Bottle *rrrBot;
                                rrrBot = rrBot->get(index).asList();
                                if(rrrBot->get(0).isInt() && numberOfInputs==1)
                                    {
                                        indexToPlot[0] = new int[rrrBot->size()];
                                        sizeIndexToPlot[0] = rrrBot->size();
                                    }
                                
                                for(int j = 0; j < rrrBot->size(); j++)
                                    {
                                        if(rrrBot->get(j).isInt())
                                            {
                                                if (numberOfInputs==1)
                                                    {
                                                        indexToPlot[0][j] = rrrBot->get(j).asInt();
                                                        //if (VERBOSE) fprintf(stderr, "MESSAGE: getting a list of indeces for a plot with a single input. Index(%d)=%d\n", j, indexToPlot[0][j]);
                                                    }
                                            }
                                        else if (rrrBot->get(j).isList())
                                            {
                                                if (rrrBot->size() == numberOfInputs)
                                                    {
                                                        //if (VERBOSE) fprintf(stderr, "MESSAGE: Getting a double list in the --index parameter! \n");
                                                        Bottle *rrrrBot;
                                                        rrrrBot = rrrBot->get(j).asList();
                                                        indexToPlot[j] = new int[rrrrBot->size()];
                                                        sizeIndexToPlot[j] = rrrrBot->size();
                                                        //if (VERBOSE) fprintf(stderr, "MESSAGE: Plot %d has %d indeces to plot! %s \n", index, sizeIndexToPlot[j], rrrrBot->toString().c_str());
                                                        for(int k = 0; k < rrrrBot->size(); k++)
                                                            {
                                                                if(rrrrBot->get(k).isInt())
                                                                    indexToPlot[j][k] = rrrrBot->get(k).asInt();
                                                            }
                                                        //if (VERBOSE) fprintf(stderr, "MESSAGE: the index to plot for input %d was set! \n", j);
                                                    }
                                                else
                                                    fprintf(stderr, "ERROR: a list of indeces was supplied but its dimension differs from the number of input ports given!\n");
                                                
                                            }

                                    }

                            }
                    }
                else if(rrBot->size() > 1 && rrBot->get(0).isInt())
                    {
                        sizeIndexToPlot[index] = rrBot->size();
                        indexToPlot[index] = new int[sizeIndexToPlot[0]];
                        for (int i = 0; i < rrBot->size(); i++)
                            indexToPlot[index][i] = rrBot->get(i).asInt();
                    }
                else
                    {
                        sizeIndexToPlot[0] = 1;
                        indexToPlot[0] = new int;
                        indexToPlot[0][0] = index;
                        fprintf(stderr, "WARNING: Ignoring option index since dimensions do not match\n");
                    }
            }
        else if (rf->find("index").isInt())
            {
                //if (VERBOSE) fprintf(stderr, "MESSAGE: was not a list. Initializing with 0\n");
                sizeIndexToPlot[0] = 1;
                indexToPlot[0] = new int;
                indexToPlot[0][0] = rf->find("index").asInt();
            }
        else
            {
                //if (VERBOSE) fprintf(stderr, "MESSAGE: was not a list. Initializing with 0\n");
                sizeIndexToPlot[0] = 1;
                indexToPlot[0] = new int;
                indexToPlot[0][0] = index;
            }

        if (indexToPlot == NULL)
            {
                //if (VERBOSE) fprintf(stderr, "MESSAGE: was not initialized. Initializing with 0\n");
                sizeIndexToPlot[0] = 1;
                indexToPlot[0] = new int;
                indexToPlot[0][0] = 0;
            }
        
        //if (VERBOSE) fprintf(stderr, "Plot %d has %d input/s and the associated indeces to plot are: \n", index, numberOfInputs);
        //for (int k = 0; k < numberOfInputs; k++)
        //    {
        //      if (VERBOSE) fprintf(stderr, "Input (%d) index/indeces:", k);
        //     for (int i = 0; i < sizeIndexToPlot[k]; i++)
        //          if (VERBOSE) fprintf(stderr, " %d ", indexToPlot[k][i]);
        //       if (VERBOSE) fprintf(stderr, "\n");
        //  }
        
        plot[index]->setInputPortIndex((int**)indexToPlot, (int*)sizeIndexToPlot, (int)numberOfInputs);

    }
コード例 #30
0
bool learnPrimitive::updateProtoAction(ResourceFinder &rf){
    Bottle bOutput;

    //1. Protoaction
    Bottle bProtoAction = rf.findGroup("Proto_Action");

    if (!bProtoAction.isNull())
    {
        Bottle * bProtoActionName = bProtoAction.find("protoActionName").asList();
        Bottle * bProtoActionEnd = bProtoAction.find("protoActionEnd").asList();
        Bottle * bProtoActionSpeed = bProtoAction.find("protoActionSpeed").asList();

       if(bProtoActionName->isNull() || bProtoActionEnd->isNull() || bProtoActionSpeed->isNull()){
           yError() << " [updateProtoAction] : one of the protoAction conf is null : protoActionName, protoActionEnd, protoActionSpeed" ;
            return false ;
        }


        int protoActionSize = -1 ;
        if(bProtoActionName->size() == bProtoActionEnd->size() && bProtoActionEnd->size() == bProtoActionSpeed->size()){
            protoActionSize =  bProtoActionName->size() ;
        } else {
            yError() << " [updateProtoAction] : one of the protoAction conf has different size!" ;
            return false ;
        }

        if(protoActionSize == 0) {
            yWarning() << " [updateProtoAction] : there is no protoaction defined at startup!" ;
        } else {

            for(int i = 0; i < protoActionSize ; i++) {

                //insert protoaction even if already there
                yInfo() << "ProtoAction added : (" << bProtoActionName->get(i).asString() << ", " << bProtoActionEnd->get(i).asInt() << ", " << bProtoActionSpeed->get(i).asDouble() << ")" ;
                mProtoActionEnd[bProtoActionName->get(i).asString()] = bProtoActionEnd->get(i).asInt();
                mProtoActionSpeed[bProtoActionName->get(i).asString()] = bProtoActionSpeed->get(i).asDouble();
            }
        }
    } else {
        yError() << " error in learnPrimitive::updateProtoAction | Proto_Action is NOT defined in the learnPrimitive.ini";
        return false;
    }

    //2. Effect of bodypart
    Bottle bBodyPart = rf.findGroup("BodyPart");

    if (!bBodyPart.isNull())
    {
        Bottle * bBodyPartName = bBodyPart.find("bodyPartName").asList();
        Bottle * bBodyPartEnd = bBodyPart.find("bodyPartProtoEnd").asList();
        Bottle * bBodyPartSpeed = bBodyPart.find("bodyPartProtoSpeed").asList();
        
        //Crash if no match : isnull is not good for that

        if(bBodyPartName->isNull() || bBodyPartEnd->isNull() || bBodyPartSpeed->isNull()){
            yError() << "[updateProtoAction] : one of the bodyPartProto conf is null : bodyPartName, bodyPartProtoEnd, bodyPartProtoSpeed" ;
            return false ;
        }


        int bodyPartSize = -1 ;
        if(bBodyPartName->size() == bBodyPartEnd->size() && bBodyPartEnd->size() == bBodyPartSpeed->size()){
            bodyPartSize =  bBodyPartName->size() ;
        } else {
            yError() << "[updateProtoAction] : one of the bodyPartProto conf has different size!" ;
            return false ;
        }

        if(bodyPartSize == 0) {
            yWarning() << "[updateProtoAction] : there is no bodyPartProto defined at startup!" ;
        } else {
            for(int i = 0; i < bodyPartSize ; i++) {

                //insert protoaction even if already there
                yInfo() << "bodyPartProto added : (" << bBodyPartName->get(i).asString() << ", " << bBodyPartEnd->get(i).asInt() << ", " << bBodyPartSpeed->get(i).asDouble() << ")" ;
                mBodyPartEnd[bBodyPartName->get(i).asString()] = bBodyPartEnd->get(i).asInt();
                mBodyPartSpeed[bBodyPartName->get(i).asString()] = bBodyPartSpeed->get(i).asDouble();
            }
        }
    } else {
        yError() << " error in learnPrimitive::updateProtoAction | BodyPart is NOT defined in the learnPrimitive.ini";
        return false;
    }

    return true;
}