Beispiel #1
0
    void testToString()
    {
        {
            report(0, "testing toString int");
            bool ok = true;
            VectorOf<int> vec;
            std::string strToCheck = "0 1 2 3 4 5 6 7 8 9";
            for (size_t i=0; i<10; i++)
            {
                vec.push_back(i);
            }

            ok = vec.toString() == strToCheck;
            checkTrue(ok, "string correctly formatted");
        }

        {
            report(0, "testing toString double");
            bool ok = true;
            VectorOf<double> vec;
            std::string strToCheck = " 0.000000\t 1.000000\t 2.000000\t 3.000000\t 4.000000\t"
                                     " 5.000000\t 6.000000\t 7.000000\t 8.000000\t 9.000000";
            for (size_t i=0; i<10; i++)
            {
                vec.push_back(i);
            }

            ok = vec.toString() == strToCheck;
            checkTrue(ok, "string correctly formatted");
        }

    }
bool imuIdentifierThread::goHome()
{
    setHeadCtrlModes("position");
    // Vector pos0(6,0.0);
    // iposH -> positionMove(pos0.data());

    VectorOf<int> jointsToSet;
    jointsToSet.push_back(0);
    jointsToSet.push_back(1);
    jointsToSet.push_back(2);
    Vector poss(3,0.0);

    iposH -> positionMove(jointsToSet.size(),
                          jointsToSet.getFirst(),
                          poss.data());

    return true;
}
bool imuIdentifierThread::setHeadCtrlModes(const string &_s)
{
    printMessage(1,"Setting %s mode for head joints..\n",_s.c_str());

    if (_s!="position" && _s!="velocity")
        return false;

    VectorOf<int> jointsToSet;
    jointsToSet.push_back(0);
    jointsToSet.push_back(1);
    jointsToSet.push_back(2);
    VectorOf<int> modes;

    if (_s=="position")
    {
        modes.push_back(VOCAB_CM_POSITION);
        modes.push_back(VOCAB_CM_POSITION);
        modes.push_back(VOCAB_CM_POSITION);
    }
    else if (_s=="velocity")
    {
        modes.push_back(VOCAB_CM_VELOCITY);
        modes.push_back(VOCAB_CM_VELOCITY);
        modes.push_back(VOCAB_CM_VELOCITY);
    }

    imodH -> setControlModes(jointsToSet.size(),
                             jointsToSet.getFirst(),
                             modes.getFirst());

    Time::delay(0.1);

    return true;
}
void Controller::setJointsCtrlMode()
{
    if (jointsToSet.size()==0)
        return;

    VectorOf<int> modes;
    for (size_t i=0; i<jointsToSet.size(); i++)
    {
        if (jointsToSet[i]<eyesJoints[0])
        {
            if (neckPosCtrlOn)
                modes.push_back(VOCAB_CM_POSITION_DIRECT);
            else
                modes.push_back(VOCAB_CM_VELOCITY);
        }
        else
            modes.push_back(VOCAB_CM_MIXED);
    }

    modHead->setControlModes(jointsToSet.size(),jointsToSet.getFirst(),
                             modes.getFirst());
}
bool reactCtrlThread::setCtrlModes(const VectorOf<int> &jointsToSet,
                                   const string &_p, const string &_s)
{
    if (_s!="position" && _s!="velocity")
        return false;

    if (jointsToSet.size()==0)
        return true;

    VectorOf<int> modes;
    for (size_t i=0; i<jointsToSet.size(); i++)
    {
        if (_s=="position")
        {
            modes.push_back(VOCAB_CM_POSITION);
        }
        else if (_s=="velocity")
        {
            modes.push_back(VOCAB_CM_VELOCITY);
        }
    }

    if (_p=="arm")
    {
        imodA->setControlModes(jointsToSet.size(),
                               jointsToSet.getFirst(),
                               modes.getFirst());
    }
    else if (_p=="torso")
    {
        imodT->setControlModes(jointsToSet.size(),
                               jointsToSet.getFirst(),
                               modes.getFirst());
    }
    else
        return false;

    return true;
}
bool reactCtrlThread::areJointsHealthyAndSet(VectorOf<int> &jointsToSet,
                                             const string &_p, const string &_s)
{
    VectorOf<int> modes;
    if (_p=="arm")
    {
        modes=encsA->size();
        imodA->getControlModes(modes.getFirst());
    }
    else if (_p=="torso")
    {
        modes=encsT->size();
        imodT->getControlModes(modes.getFirst());
    }
    else
        return false;

    for (size_t i=0; i<modes.size(); i++)
    {
        if ((modes[i]==VOCAB_CM_HW_FAULT) || (modes[i]==VOCAB_CM_IDLE))
            return false;

        if (_s=="velocity")
        {
            if (modes[i]!=VOCAB_CM_MIXED || modes[i]!=VOCAB_CM_VELOCITY)
                jointsToSet.push_back(i);
        }
        else if (_s=="position")
        {
            if (modes[i]!=VOCAB_CM_MIXED || modes[i]!=VOCAB_CM_POSITION)
                jointsToSet.push_back(i);
        }

    }

    return true;
}
bool imuIdentifierThread::processWayPoint()
{
    processIMU();
    
    if (wayPoints[currentWaypoint].name == "START     " ||
        wayPoints[currentWaypoint].name == "END       " ||
        wayPoints[currentWaypoint].name == "MIDDLE    " )
    {
        if (posCtrlFlag)
        {
            printMessage(1,"Putting head in home position..\n");
            goHome();
            posCtrlFlag = false;
        }

        if (yarp::os::Time::now() - timeNow > CTRL_PERIOD)
        {
            posCtrlFlag = true;
            return false;
        }
    }
    else
    {
        Vector jls = wayPoints[currentWaypoint].jntlims;
        Vector vls = wayPoints[currentWaypoint].vels;
        bool flag = false;

        iencsH->getEncoders(encsH->data());
        yarp::sig::Vector head = *encsH;

        // ivelH -> velocityMove(vls.data());

        VectorOf<int> jointsToSet;
        jointsToSet.push_back(0);
        jointsToSet.push_back(1);
        jointsToSet.push_back(2);
    
        ivelH -> velocityMove(jointsToSet.size(),
                              jointsToSet.getFirst(),
                              vls.data());

        for (int i = 0; i < 3; i++)
        {
            if      (vls(i) > 0.0)
            {
                if (jls(i) - head(i) > 0.0)
                {
                    flag = true;
                }
            }
            else if (vls(i) < 0.0)
            {
                if (jls(i) - head(i) < 0.0)
                {
                    flag = true;
                }
            }
        }

        return flag;
    }

    return true;
}
Beispiel #8
0
    bool configure(ResourceFinder &rf)
    {
        string name=rf.check("name",Value("teleop-icub")).asString().c_str();
        string robot=rf.check("robot",Value("icub")).asString().c_str();
        string geomagic=rf.check("geomagic",Value("geomagic")).asString().c_str();
        double Tp2p=rf.check("Tp2p",Value(1.0)).asDouble();
        part=rf.check("part",Value("right_arm")).asString().c_str();
        simulator=rf.check("simulator",Value("off")).asString()=="on";
        gaze=rf.check("gaze",Value("off")).asString()=="on";
        minForce=fabs(rf.check("min-force-feedback",Value(3.0)).asDouble());
        maxForce=fabs(rf.check("max-force-feedback",Value(15.0)).asDouble());
        bool torso=rf.check("torso",Value("on")).asString()=="on";

        Property optGeo("(device hapticdeviceclient)");
        optGeo.put("remote",("/"+geomagic).c_str());
        optGeo.put("local",("/"+name+"/geomagic").c_str());
        if (!drvGeomagic.open(optGeo))
            return false;
        drvGeomagic.view(igeo);

        if (simulator)
        {
            simPort.open(("/"+name+"/simulator:rpc").c_str());
            if (!Network::connect(simPort.getName().c_str(),"/icubSim/world"))
            {
                yError("iCub simulator is not running!");
                drvGeomagic.close();
                simPort.close();
                return false;
            }
        }

        if (gaze)
        {
            Property optGaze("(device gazecontrollerclient)");
            optGaze.put("remote","/iKinGazeCtrl");
            optGaze.put("local",("/"+name+"/gaze").c_str());
            if (!drvGaze.open(optGaze))
            {
                drvGeomagic.close();
                simPort.close();
                return false;
            }
            drvGaze.view(igaze);
        }

        Property optCart("(device cartesiancontrollerclient)");
        optCart.put("remote",("/"+robot+"/cartesianController/"+part).c_str());
        optCart.put("local",("/"+name+"/cartesianController/"+part).c_str());
        if (!drvCart.open(optCart))
        {
            drvGeomagic.close();
            if (simulator)
                simPort.close();
            if (gaze)
                drvGaze.close();            
            return false;
        }
        drvCart.view(iarm);

        Property optHand("(device remote_controlboard)");
        optHand.put("remote",("/"+robot+"/"+part).c_str());
        optHand.put("local",("/"+name+"/"+part).c_str());
        if (!drvHand.open(optHand))
        {
            drvGeomagic.close();
            if (simulator)
                simPort.close();
            if (gaze)
                drvGaze.close();
            drvCart.close();
            return false;
        }
        drvHand.view(imod);
        drvHand.view(ipos);
        drvHand.view(ivel);

        iarm->storeContext(&startup_context);
        iarm->restoreContext(0);

        Vector dof(10,1.0);
        if (!torso)
            dof[0]=dof[1]=dof[2]=0.0;
        else
            dof[1]=0.0;
        iarm->setDOF(dof,dof);
        iarm->setTrajTime(Tp2p);
        
        Vector accs,poss;
        for (int i=0; i<9; i++)
        {
            joints.push_back(7+i);
            modes.push_back(VOCAB_CM_POSITION);
            accs.push_back(1e9);
            vels.push_back(100.0);
            poss.push_back(0.0);
        }
        poss[0]=20.0;
        poss[1]=70.0;
        
        imod->setControlModes(joints.size(),joints.getFirst(),modes.getFirst());
        ipos->setRefAccelerations(joints.size(),joints.getFirst(),accs.data());
        ipos->setRefSpeeds(joints.size(),joints.getFirst(),vels.data());
        ipos->positionMove(joints.size(),joints.getFirst(),poss.data());

        joints.clear();
        modes.clear();
        vels.clear();
        for (int i=2; i<9; i++)
        {
            joints.push_back(7+i);
            modes.push_back(VOCAB_CM_VELOCITY);
            vels.push_back(40.0);
        }
        vels[vels.length()-1]=100.0;
        
        s0=s1=idle;
        c0=c1=0;
        onlyXYZ=true;
        
        stateStr[idle]="idle";
        stateStr[triggered]="triggered";
        stateStr[running]="running";

        Matrix T=zeros(4,4);
        T(0,1)=1.0;
        T(1,2)=1.0;
        T(2,0)=1.0;
        T(3,3)=1.0;
        igeo->setTransformation(SE3inv(T));
        igeo->setCartesianForceMode();
        igeo->getMaxFeedback(maxFeedback);
        
        Tsim=zeros(4,4);
        Tsim(0,1)=-1.0;
        Tsim(1,2)=1.0;  Tsim(1,3)=0.5976;
        Tsim(2,0)=-1.0; Tsim(2,3)=-0.026;
        Tsim(3,3)=1.0;

        pos0.resize(3,0.0);
        rpy0.resize(3,0.0);

        x0.resize(3,0.0);
        o0.resize(4,0.0);

        if (simulator)
        {
            Bottle cmd,reply;
            cmd.addString("world");
            cmd.addString("mk");
            cmd.addString("ssph");
                
            // radius
            cmd.addDouble(0.02);

            // position
            cmd.addDouble(0.0);
            cmd.addDouble(0.0);
            cmd.addDouble(0.0);
                
            // color
            cmd.addInt(1);
            cmd.addInt(0);
            cmd.addInt(0);

            // collision
            cmd.addString("FALSE");

            simPort.write(cmd,reply);
        }

        forceFbPort.open(("/"+name+"/force-feedback:i").c_str());
        feedback.resize(3,0.0);

        return true;
    }