예제 #1
0
    void handHandler(const bool b)
    {
        if (b)
        {
            if (s1==idle)
                s1=triggered;
            else if (s1==triggered)
            {
                if (++c1*getPeriod()>0.5)
                {
                    imod->setControlModes(joints.size(),joints.getFirst(),modes.getFirst());
                    s1=running;
                }
            }
            else
                ivel->velocityMove(joints.size(),joints.getFirst(),vels.data());
        }
        else
        {
            if (s1==triggered)
                vels=-1.0*vels;

            if (c1!=0)
                ivel->stop(joints.size(),joints.getFirst());

            s1=idle;
            c1=0;
        }
    }
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;
}
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;
}
예제 #4
0
    bool close()
    {
        iarm->stopControl();
        iarm->restoreContext(startup_context);
        drvCart.close();

        ivel->stop(joints.size(),joints.getFirst());
        for (size_t i=0; i<modes.size(); i++)
            modes[i]=VOCAB_CM_POSITION;
        imod->setControlModes(joints.size(),joints.getFirst(),modes.getFirst());
        drvHand.close();

        if (simulator)
        {
            Bottle cmd,reply;
            cmd.addString("world");
            cmd.addString("del");
            cmd.addString("all");
            simPort.write(cmd,reply);

            simPort.close();
        }

        if (gaze)
        {
            igaze->stopControl();
            drvGaze.close();
        }

        igeo->stopFeedback();
        igeo->setTransformation(eye(4,4));
        drvGeomagic.close();
        forceFbPort.close();

        return true;
    }
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 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;
}
예제 #7
0
    void checkSendReceiveInt()
    {
        report(0, "check VectorO<int> send receive");

        {
            Port portIn;
            Port portOut;

            portOut.open("/harness_sig/vtest/o");
            portIn.open("/harness_sig/vtest/i");

            Network::connect("/harness_sig/vtest/o", "/harness_sig/vtest/i");

            portOut.enableBackgroundWrite(true);


            VectorOf<int> vector;
            vector.resize(10);
            for (unsigned int k = 0; k < vector.size(); k++)
            {
                vector[k] = k;
            }

            bool success = true;
            portOut.write(vector);

            VectorOf<int> tmp;
            portIn.read(tmp);

            //compare vector and tmp
            if (tmp.size() != vector.size())
            {
                success = false;
            }
            else
            {
                for (unsigned int k = 0; k < vector.size(); k++)
                {
                    if (tmp[k] != vector[k])
                        success = false;
                }
            }

            checkTrue(success, "VectorOf<int> was sent and received correctly");
            portOut.interrupt();
            portOut.close();
            portIn.interrupt();
            portIn.close();
        }

        report(0, "check VectorOf<int> bottle compatibility");
        {
            //write the same vector again and receive it as a bottle
            Port portIn;
            Port portOut;
            bool success = true;

            portOut.open("/harness_sig/vtest/o");
            portIn.open("/harness_sig/vtest/i");

            Network::connect("/harness_sig/vtest/o", "/harness_sig/vtest/i");

            portOut.enableBackgroundWrite(true);


            VectorOf<int> vector;
            vector.resize(10);
            for (unsigned int k = 0; k < vector.size(); k++)
            {
                vector[k] = k;
            }
            portOut.write(vector);
            Bottle tmp2;
            success = portIn.read(tmp2);
            checkTrue(success,"correctly read from the port");

            //compare vector and tmp
            success = true;
            if (tmp2.size() != vector.size())
            {
                success = false;
            }
            else
            {
                for (unsigned int k = 0; k < vector.size(); k++)
                {
                    if (tmp2.get(k).asInt32() != vector[k])
                        success = false;
                }
            }

            checkTrue(success, "VectorOf<int> was received correctly in a Bottle");
            portOut.interrupt();
            portOut.close();
            portIn.interrupt();
            portIn.close();
        }
    }
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;
}
예제 #9
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;
    }