C++ IEncoders::getAxes方法代码示例

Matrix alignJointsBounds(iKinChain *chain, PolyDriver *drvTorso, PolyDriver *drvHead,
                         const double eyeTiltMin, const double eyeTiltMax)
{
    IEncoders      *encs;
    IControlLimits *lims;

    double min, max;
    int nJointsTorso=1;

    if (drvTorso!=NULL)
    {
        drvTorso->view(encs);
        drvTorso->view(lims);        
        encs->getAxes(&nJointsTorso);

        for (int i=0; i<nJointsTorso; i++)
        {   
            lims->getLimits(i,&min,&max);
        
            (*chain)[nJointsTorso-1-i].setMin(CTRL_DEG2RAD*min); // reversed order
            (*chain)[nJointsTorso-1-i].setMax(CTRL_DEG2RAD*max);
        }
    }

    drvHead->view(encs);
    drvHead->view(lims);
    int nJointsHead;
    encs->getAxes(&nJointsHead);
    Matrix lim(nJointsHead,2);

    for (int i=0; i<nJointsHead; i++)
    {   
        lims->getLimits(i,&min,&max);

        // limit eye's tilt due to eyelids
        if (i==2)
        {
            min=std::max(min,eyeTiltMin);
            max=std::min(max,eyeTiltMax);
        }

        lim(i,0)=CTRL_DEG2RAD*min;
        lim(i,1)=CTRL_DEG2RAD*max;

        // just one eye's got only 5 dofs
        if (i<nJointsHead-1)
        {
            (*chain)[nJointsTorso+i].setMin(lim(i,0));
            (*chain)[nJointsTorso+i].setMax(lim(i,1));
        }
    }

    return lim;
}

    bool threadInit()
    {
        //initialize here variables
        printf("ControlThread:starting\n");
        
        Property options;
        options.put("device", "remote_controlboard");
        options.put("local", "/local/head");
        
        //substitute icubSim with icub for use with the real robot
        options.put("remote", "/icubSim/head");
        
        dd.open(options);

        dd.view(iencs);
        dd.view(ivel);

        if ( (!iencs) || (!ivel) )
            return false;
        
        int joints;
   
        iencs->getAxes(&joints);
    
        encoders.resize(joints);
        commands.resize(joints);

        commands=10000;
        ivel->setRefAccelerations(commands.data());

        count=0;
        return true;
    }

    void _send(const ActionItem *x)
    {
        if (!connected)
        {
            cerr<<"Error: not connected to control board skipping"<<endl;
            return;
        }

        int size=x->getCmd().size();
        int offset=x->getOffset();
        double time=x->getTime();
        int nJoints=0;

        enc->getAxes(&nJoints);
        if ((offset+size)>nJoints)
        {
            cerr<<"Error: detected possible overflow, skipping"<<endl;
            cerr<<"For debug --> joints: "<<nJoints<< " off: "<<offset<<" cmd length: "<<size<<endl;
            return;
        }

        Vector disp(size);

        if (time==0)
        {
            return;
        }

        for (size_t i=0; i<disp.length(); i++)
        {
            double q;


            if (!enc->getEncoder(offset+i,&q))
            {
                cerr<<"Error: encoders timed out, cannot rely on encoder feedback, aborted"<<endl;
                return;
            }

            disp[i]=x->getCmd()[i]-q;

            if (disp[i]<0.0)
                disp[i]=-disp[i];
        }

        for (size_t i=0; i<disp.length(); i++)
        {
            pos->setRefSpeed(offset+i,disp[i]/time);
            pos->positionMove(offset+i,x->getCmd()[i]);
        }

        cout << "Script port: " << const_cast<Vector &>(x->getCmd()).toString() << endl;
    }

    bool configure(ResourceFinder &rf)
    {
        printf(" Starting Configure\n");
        this->rf=&rf;

        //****************** PARAMETERS ******************
        robot     = rf.check("robot",Value("icub")).asString().c_str();
        name      = rf.check("name",Value("demoINNOROBO")).asString().c_str();
        arm       = rf.check("arm",Value("right_arm")).asString().c_str();
        verbosity = rf.check("verbosity",Value(0)).asInt();    
        rate      = rf.check("rate",Value(0.5)).asDouble();

        if (arm!="right_arm" && arm!="left_arm")
        {
            printMessage(0,"ERROR: arm was set to %s, putting it to right_arm\n",arm.c_str());
            arm="right_arm";
        }

        printMessage(1,"Parameters correctly acquired\n");
        printMessage(1,"Robot: %s \tName: %s \tArm: %s\n",robot.c_str(),name.c_str(),arm.c_str());
        printMessage(1,"Verbosity: %i \t Rate: %g \n",verbosity,rate);

        //****************** DETECTOR ******************
        certainty     = rf.check("certainty", Value(10.0)).asInt();
        strCascade = rf.check("cascade", Value("haarcascade_frontalface_alt.xml")).asString().c_str();

        detectorL=new Detector();
        detectorL->open(certainty,strCascade);
        detectorR=new Detector();
        detectorR->open(certainty,strCascade);

        printMessage(1,"Detectors opened\n");

        //****************** DRIVERS ******************
        Property optionArm("(device remote_controlboard)");
        optionArm.put("remote",("/"+robot+"/"+arm).c_str());
        optionArm.put("local",("/"+name+"/"+arm).c_str());
        if (!drvArm.open(optionArm))
        {
            printf("Position controller not available!\n");
            return false;
        }

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

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

        // quitting conditions
        bool andArm=drvArm.isValid()==drvCart.isValid()==drvGaze.isValid();
        if (!andArm)
        {
            printMessage(0,"Something wrong occured while configuring drivers... quitting!\n");
            return false;
        }

        drvArm.view(iencs);
        drvArm.view(iposs);
        drvArm.view(ictrl);
        drvArm.view(iimp);
        drvCart.view(iarm);
        drvGaze.view(igaze);
        iencs->getAxes(&nEncs);

        iimp->setImpedance(0,  0.4, 0.03);
        iimp->setImpedance(1, 0.35, 0.03);
        iimp->setImpedance(2, 0.35, 0.03);
        iimp->setImpedance(3,  0.2, 0.02);
        iimp->setImpedance(4,  0.2, 0.00);

        ictrl -> setImpedancePositionMode(0);
        ictrl -> setImpedancePositionMode(1);
        ictrl -> setImpedancePositionMode(2);
        ictrl -> setImpedancePositionMode(3);
        ictrl -> setImpedancePositionMode(2);
        ictrl -> setImpedancePositionMode(4);

        igaze -> storeContext(&contextGaze);
        igaze -> setSaccadesStatus(false);
        igaze -> setNeckTrajTime(0.75);
        igaze -> setEyesTrajTime(0.5);

        iarm -> storeContext(&contextCart);

        printMessage(1,"Drivers opened\n");

        //****************** PORTS ******************
//.........这里部分代码省略.........

    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;
    }

    void _send(const ActionItem *x)
    {
        if (!connected)
        {
            cerr<<"Error: not connected to control board skipping"<<endl;
            return;
        }

        int size=x->getCmd().size();
        int offset=x->getOffset();
        double time=x->getTime();
        int nJoints=0;

        enc->getAxes(&nJoints);
        if ((offset+size)>nJoints)
        {
            cerr<<"Error: detected possible overflow, skipping"<<endl;
            cerr<<"For debug --> joints: "<<nJoints<< " off: "<<offset<<" cmd length: "<<size<<endl;
            return;
        }

        Vector disp(size);

        if (time==0)
        {
            return;
        }

        for (size_t i=0; i<disp.length(); i++)
        {
            double q;

           
            if (!enc->getEncoder(offset+i,&q))
            {
                 cerr<<"Error: encoders timed out, cannot rely on encoder feedback, aborted"<<endl;
                 return;
            }
                
            disp[i]=x->getCmd()[i]-q;

            if (disp[i]<0.0)
                disp[i]=-disp[i];
        }

        // don't blend together the two "for"
        // since we have to enforce the modes on the whole
        // prior to commanding the joints
        std::vector<int>    joints;
        std::vector<int>    modes;
        std::vector<double> speeds;
        std::vector<double> positions;

        for (size_t i=0; i<disp.length(); i++)
        {
            joints.push_back(offset+i);
            speeds.push_back(disp[i]/time);
            modes.push_back(VOCAB_CM_POSITION);
            positions.push_back(x->getCmd()[i]);
        }

        mode->setControlModes(disp.length(), joints.data(), modes.data());
        yarp::os::Time::delay(0.01);  // give time to update control modes value
        mode->getControlModes(disp.length(), joints.data(), modes.data());
        for (size_t i=0; i<disp.length(); i++)
        {
            if(modes[i] != VOCAB_CM_POSITION)
            {
                yError() << "Joint " << i << " not in position mode";
            }
        }
        pos->setRefSpeeds(disp.length(), joints.data(), speeds.data());
        pos->positionMove(disp.length(), joints.data(), positions.data());

        cout << "Script port: " << x->getCmd().toString() << endl;
    }

Controller::Controller(PolyDriver *_drvTorso, PolyDriver *_drvHead, exchangeData *_commData,
                       const bool _neckPosCtrlOn, const double _neckTime, const double _eyesTime,
                       const double _minAbsVel, const unsigned int _period) :
                       RateThread(_period), drvTorso(_drvTorso),           drvHead(_drvHead),
                       commData(_commData), neckPosCtrlOn(_neckPosCtrlOn), neckTime(_neckTime),
                       eyesTime(_eyesTime), minAbsVel(_minAbsVel),         period(_period),
                       Ts(_period/1000.0),  printAccTime(0.0)
{
    // Instantiate objects
    neck=new iCubHeadCenter(commData->head_version>1.0?"right_v2":"right");
    eyeL=new iCubEye(commData->head_version>1.0?"left_v2":"left");
    eyeR=new iCubEye(commData->head_version>1.0?"right_v2":"right");

    // release links
    neck->releaseLink(0); eyeL->releaseLink(0); eyeR->releaseLink(0);
    neck->releaseLink(1); eyeL->releaseLink(1); eyeR->releaseLink(1);
    neck->releaseLink(2); eyeL->releaseLink(2); eyeR->releaseLink(2);

    // Get the chain objects
    chainNeck=neck->asChain();
    chainEyeL=eyeL->asChain();
    chainEyeR=eyeR->asChain();

    // add aligning matrices read from configuration file
    getAlignHN(commData->rf_cameras,"ALIGN_KIN_LEFT",eyeL->asChain());
    getAlignHN(commData->rf_cameras,"ALIGN_KIN_RIGHT",eyeR->asChain());

    // overwrite aligning matrices iff specified through tweak values
    if (commData->tweakOverwrite)
    {
        getAlignHN(commData->rf_tweak,"ALIGN_KIN_LEFT",eyeL->asChain());
        getAlignHN(commData->rf_tweak,"ALIGN_KIN_RIGHT",eyeR->asChain());
    }

    // read number of joints
    if (drvTorso!=NULL)
    {
        IEncoders *encTorso; drvTorso->view(encTorso);
        encTorso->getAxes(&nJointsTorso);
    }
    else
        nJointsTorso=3;
    
    IEncoders *encHead; drvHead->view(encHead);
    encHead->getAxes(&nJointsHead);

    drvHead->view(modHead);
    drvHead->view(posHead);
    drvHead->view(velHead);

    // if requested check if position control is available
    if (neckPosCtrlOn)
    {
        neckPosCtrlOn=drvHead->view(posNeck);
        yInfo("### neck control - requested POSITION mode: IPositionDirect [%s] => %s mode selected",
              neckPosCtrlOn?"available":"not available",neckPosCtrlOn?"POSITION":"VELOCITY");
    }
    else
        yInfo("### neck control - requested VELOCITY mode => VELOCITY mode selected");

    // joints bounds alignment
    lim=alignJointsBounds(chainNeck,drvTorso,drvHead,commData->eyeTiltMin,commData->eyeTiltMax);

    // read starting position
    fbTorso.resize(nJointsTorso,0.0);
    fbHead.resize(nJointsHead,0.0);

    // exclude acceleration constraints by fixing
    // thresholds at high values
    Vector a_robHead(nJointsHead,1e9);
    velHead->setRefAccelerations(a_robHead.data());

    copyJointsBounds(chainNeck,chainEyeL);
    copyJointsBounds(chainEyeL,chainEyeR);

    // find minimum allowed vergence
    startupMinVer=lim(nJointsHead-1,0);
    findMinimumAllowedVergence();

    // reinforce vergence min bound
    lim(nJointsHead-1,0)=commData->get_minAllowedVergence();
    getFeedback(fbTorso,fbHead,drvTorso,drvHead,commData);

    fbNeck=fbHead.subVector(0,2);
    fbEyes=fbHead.subVector(3,5);
    qdNeck.resize(3,0.0); qdEyes.resize(3,0.0);
    vNeck.resize(3,0.0);  vEyes.resize(3,0.0);

    // Set the task execution time
    setTeyes(eyesTime);
    setTneck(neckTime);

    mjCtrlNeck=new minJerkVelCtrlForIdealPlant(Ts,fbNeck.length());
    mjCtrlEyes=new minJerkVelCtrlForIdealPlant(Ts,fbEyes.length());
    IntState=new Integrator(Ts,fbHead,lim);
    IntPlan=new Integrator(Ts,fbNeck,lim.submatrix(0,2,0,1));
    
    v.resize(nJointsHead,0.0);

    neckJoints.resize(3);
//.........这里部分代码省略.........

int main(int argc, char *argv[]) 
{
    // just list the devices if no argument given
    if (argc <= 2) {
        printf("You can call %s like this:\n", argv[0]);
        printf("   %s --robot ROBOTNAME --OPTION VALUE ...\n", argv[0]);
        printf("For example:\n");
        printf("   %s --robot icub --local /talkto/james --remote /controlboard/rpc\n", argv[0]);
        printf("Here are devices listed for your system:\n");
        printf("%s", Drivers::factory().toString().c_str());
        return 0;
    }

    // get command line options
    Property options;
    options.fromCommand(argc, argv);
    if (!options.check("robot") || !options.check("part")) {
        printf("Missing either --robot or --part options\n");
        return 0;
    }

    Network::init();
	Time::turboBoost();
    
    std::string name;
    Value& v = options.find("robot");
    Value& part = options.find("part");

    Value *val;
    if (!options.check("device", val)) {
        options.put("device", "remote_controlboard");
    }
    if (!options.check("local", val)) {
		name="/"+std::string(v.asString().c_str())+"/"+std::string(part.asString().c_str())+"/simpleclient";
        //sprintf(&name[0], "/%s/%s/client", v.asString().c_str(), part.asString().c_str());
        options.put("local", name.c_str());
    }
    if (!options.check("remote", val)) {
        name="/"+std::string(v.asString().c_str())+"/"+std::string(part.asString().c_str());    
		//sprintf(&name[0], "/%s/%s", v.asString().c_str(), part.asString().c_str());
        options.put("remote", name.c_str());
    }

	fprintf(stderr, "%s", options.toString().c_str());

    
    // create a device 
    PolyDriver dd(options);
    if (!dd.isValid()) {
        printf("Device not available.  Here are the known devices:\n");
        printf("%s", Drivers::factory().toString().c_str());
        Network::fini();
        return 1;
    }

    IPositionControl *pos;
    IPositionDirect  *posDir;
    IVelocityControl *vel;
    IEncoders *enc;
    IPidControl *pid;
    IAmplifierControl *amp;
    IControlLimits *lim;
//    IControlMode *icm;
    IControlMode2 *iMode2;
    ITorqueControl *itorque;
    IOpenLoopControl *iopenloop;
	IImpedanceControl *iimp;
    IInteractionMode *iInteract;

    bool ok;
    ok = dd.view(pos);
    ok &= dd.view(vel);
    ok &= dd.view(enc);
    ok &= dd.view(pid);
    ok &= dd.view(amp);
    ok &= dd.view(lim);
//    ok &= dd.view(icm);
    ok &= dd.view(itorque);
    ok &= dd.view(iopenloop);
	ok &= dd.view(iimp);
    ok &= dd.view(posDir);
    ok &= dd.view(iMode2);
    ok &= dd.view(iInteract);

    if (!ok) {
        printf("Problems acquiring interfaces\n");
        return 1;
    }

    pos->getAxes(&jnts);
    printf("Working with %d axes\n", jnts);
    double *tmp = new double[jnts];

    printf("Device active...\n");
    while (dd.isValid()) {
        std::string s;
        s.resize(1024);
        
        printf("-> ");
        char c = 0;
//.........这里部分代码省略.........

int main(int argc, char **argv)
{
    Network yarp;

    printf("Going to stress rpc connections to the robot\n");
    printf("Run as --id unique-id\n");
    printf("Optionally:\n");
    printf("--part robot-part\n");
    printf("--prot protocol\n");
    printf("--time dt (seconds)\n");
    printf("--robot name \n");

    Property parameters;
    parameters.fromCommand(argc, argv);

    ConstString part=parameters.find("part").asString();
    int id=parameters.find("id").asInt();
    double time=0;
    if (parameters.check("time"))
        {
            time=parameters.find("time").asDouble();
        }
    else
        time=-1;

    ConstString protocol;
    if (parameters.check("prot"))
    {
        protocol=parameters.find("prot").asString();
    }
    else
        protocol="tcp";

    ConstString rname;
    if (parameters.check("robot"))
    {
        rname=parameters.find("robot").asString();
    }
    else
        rname="controlboard";

    PolyDriver dd;
    Property p;

    Random::seed((unsigned int)Time::now());
     
    string remote=string("/")+rname.c_str();
    if (part!="")
        {
            remote+=string("/");
            remote+=part;
        }
    string local=string("/")+string(rname.c_str());
    if (part!="")
        {
            local+=string("/");
            local+=part;
        }
    local+=string("/stress");

    stringstream lStream; 
    lStream << id;
    local += lStream.str();

    p.put("device", "remote_controlboard");
    p.put("local", local.c_str());
    p.put("remote", remote.c_str());
    p.put("carrier", protocol.c_str());
    dd.open(p);
    
    if (!dd.isValid())
    {
        fprintf(stderr, "Error, could not open controlboard\n");
        return -1;
    }

    IEncoders *ienc;
    IPositionControl *ipos;
    IAmplifierControl *iamp;
    IPidControl *ipid;
    IControlLimits *ilim;
    IControlCalibration2 *ical;

    dd.view(ienc);
    dd.view(ipos);
    dd.view(iamp);
    dd.view(ipid);
    dd.view(ilim);
    dd.view(ical);
    
    int c=100;
    int nj;
    Vector encoders;
    ienc->getAxes(&nj);
    encoders.resize(nj);

    int count=0;
    bool done=false;
    double startT=Time::now();
    double now=0;
//.........这里部分代码省略.........

bool SpringyFingersModel::calibrate(const Property &options)
{
    if (configured)
    {
        IControlMode2    *imod; driver.view(imod);
        IControlLimits   *ilim; driver.view(ilim);
        IEncoders        *ienc; driver.view(ienc);
        IPositionControl *ipos; driver.view(ipos);

        int nAxes; ienc->getAxes(&nAxes);
        Vector qmin(nAxes),qmax(nAxes),vel(nAxes),acc(nAxes);

        printMessage(1,"steering the hand to a suitable starting configuration\n");
        for (int j=7; j<nAxes; j++)
        {
            imod->setControlMode(j,VOCAB_CM_POSITION);
            ilim->getLimits(j,&qmin[j],&qmax[j]);            

            ipos->getRefAcceleration(j,&acc[j]);
            ipos->getRefSpeed(j,&vel[j]);
            
            ipos->setRefAcceleration(j,1e9);
            ipos->setRefSpeed(j,60.0);
            ipos->positionMove(j,(j==8)?qmax[j]:qmin[j]);   // thumb in opposition
        }

        printMessage(1,"proceeding with the calibration\n");
        Property &opt=const_cast<Property&>(options);
        string tag=opt.check("finger",Value("all")).asString().c_str();
        if (tag=="thumb")
        {
            calibrateFinger(fingers[0],10,qmin[10],qmax[10]);
        }
        else if (tag=="index")
        {
            calibrateFinger(fingers[1],12,qmin[12],qmax[12]);
        }
        else if (tag=="middle")
        {
            calibrateFinger(fingers[2],14,qmin[14],qmax[14]);
        }
        else if (tag=="ring")
        {
            calibrateFinger(fingers[3],15,qmin[15],qmax[15]);
        }
        else if (tag=="little")
        {
            calibrateFinger(fingers[4],15,qmin[15],qmax[15]);
        }
        else if ((tag=="all") || (tag=="all_serial"))
        {
            calibrateFinger(fingers[0],10,qmin[10],qmax[10]);
            calibrateFinger(fingers[1],12,qmin[12],qmax[12]);
            calibrateFinger(fingers[2],14,qmin[14],qmax[14]);
            calibrateFinger(fingers[3],15,qmin[15],qmax[15]);
            calibrateFinger(fingers[4],15,qmin[15],qmax[15]);
        }
        else if (tag=="all_parallel")
        {
            CalibThread thr[5];
            thr[0].setInfo(this,fingers[0],10,qmin[10],qmax[10]);
            thr[1].setInfo(this,fingers[1],12,qmin[12],qmax[12]);
            thr[2].setInfo(this,fingers[2],14,qmin[14],qmax[14]);
            thr[3].setInfo(this,fingers[3],15,qmin[15],qmax[15]);
            thr[4].setInfo(this,fingers[4],15,qmin[15],qmax[15]);

            thr[0].start(); thr[1].start(); thr[2].start();
            thr[3].start(); thr[4].start();

            bool done=false;
            while (!done)
            {
                done=true;
                for (int i=0; i<5; i++)
                {
                    done&=thr[i].isDone();
                    if (thr[i].isDone() && thr[i].isRunning())
                        thr[i].stop();
                }

                Time::delay(0.1);
            }
        }
        else
        {
            printMessage(1,"unknown finger request %s\n",tag.c_str());
            return false;
        }

        for (int j=7; j<nAxes; j++)
        {
            ipos->setRefAcceleration(j,acc[j]);
            ipos->setRefSpeed(j,vel[j]);
        }

        return true;
    }
    else
        return false;
}

bool SpringyFingersModel::fromProperty(const Property &options)
{
    Property &opt=const_cast<Property&>(options);
    if (!opt.check("name") || !opt.check("type"))
    {
        printMessage(1,"missing mandatory options \"name\" and/or \"type\"\n");
        return false;
    }

    if (configured)
        close();

    name=opt.find("name").asString().c_str();
    type=opt.find("type").asString().c_str();
    robot=opt.check("robot",Value("icub")).asString().c_str();
    carrier=opt.check("carrier",Value("udp")).asString().c_str();
    verbosity=opt.check("verbosity",Value(0)).asInt();

    string part_motor=string(type+"_arm");
    string part_analog=string(type+"_hand");

    Property prop;
    prop.put("device","remote_controlboard");
    prop.put("remote",("/"+robot+"/"+part_motor).c_str());
    prop.put("local",("/"+name+"/"+part_motor).c_str());
    if (!driver.open(prop))
        return false;
    
    port->open(("/"+name+"/"+part_analog+"/analog:i").c_str());
    string analogPortName(("/"+robot+"/"+part_analog+"/analog:o").c_str());
    if (!Network::connect(analogPortName.c_str(),port->getName().c_str(),carrier.c_str()))
    {
        printMessage(1,"unable to connect to %s\n",analogPortName.c_str());
        close();
        return false;
    }

    IEncoders *ienc; driver.view(ienc);
    int nAxes; ienc->getAxes(&nAxes);

    printMessage(1,"configuring interface-based sensors ...\n");
    Property propGen;
    propGen.put("name","In_0");
    propGen.put("size",nAxes);

    Property propThumb=propGen;  propThumb.put( "index",10);
    Property propIndex=propGen;  propIndex.put( "index",12);
    Property propMiddle=propGen; propMiddle.put("index",14);
    Property propRing=propGen;   propRing.put(  "index",15);
    Property propLittle=propGen; propLittle.put("index",15);

    bool sensors_ok=true;
    void *pIF=static_cast<void*>(ienc);
    sensors_ok&=sensIF[0].configure(pIF,propThumb);
    sensors_ok&=sensIF[1].configure(pIF,propIndex);
    sensors_ok&=sensIF[2].configure(pIF,propMiddle);
    sensors_ok&=sensIF[3].configure(pIF,propRing);
    sensors_ok&=sensIF[4].configure(pIF,propLittle);

    printMessage(1,"configuring port-based sensors ...\n");
    Property thumb_mp(  "(name Out_0) (index 1)" );
    Property thumb_ip(  "(name Out_1) (index 2)" );
    Property index_mp(  "(name Out_0) (index 4)" );
    Property index_ip(  "(name Out_1) (index 5)" );
    Property middle_mp( "(name Out_0) (index 7)" );
    Property middle_ip( "(name Out_1) (index 8)" );
    Property ring_mp(   "(name Out_0) (index 9)" );
    Property ring_pip(  "(name Out_1) (index 10)");
    Property ring_dip(  "(name Out_2) (index 11)");
    Property little_mp( "(name Out_0) (index 12)");
    Property little_pip("(name Out_1) (index 13)");
    Property little_dip("(name Out_2) (index 14)");

    void *pPort=static_cast<void*>(port);
    sensors_ok&=sensPort[0].configure(pPort,thumb_mp);
    sensors_ok&=sensPort[1].configure(pPort,thumb_ip);
    sensors_ok&=sensPort[2].configure(pPort,index_mp);
    sensors_ok&=sensPort[3].configure(pPort,index_ip);
    sensors_ok&=sensPort[4].configure(pPort,middle_mp);
    sensors_ok&=sensPort[5].configure(pPort,middle_ip);
    sensors_ok&=sensPort[6].configure(pPort,ring_mp);
    sensors_ok&=sensPort[7].configure(pPort,ring_pip);
    sensors_ok&=sensPort[8].configure(pPort,ring_dip);
    sensors_ok&=sensPort[9].configure(pPort,little_mp);
    sensors_ok&=sensPort[10].configure(pPort,little_pip);
    sensors_ok&=sensPort[11].configure(pPort,little_dip);

    if (!sensors_ok)
    {
        printMessage(1,"some errors occured\n");
        close();
        return false;
    }

    printMessage(1,"configuring fingers ...\n");
    Property thumb(opt.findGroup("thumb").toString().c_str());
    Property index(opt.findGroup("index").toString().c_str());
    Property middle(opt.findGroup("middle").toString().c_str());
    Property ring(opt.findGroup("ring").toString().c_str());
    Property little(opt.findGroup("little").toString().c_str());
//.........这里部分代码省略.........

Solver::Solver(PolyDriver *_drvTorso, PolyDriver *_drvHead, ExchangeData *_commData,
               EyePinvRefGen *_eyesRefGen, Localizer *_loc, Controller *_ctrl,
               const unsigned int _period) :
               RateThread(_period), drvTorso(_drvTorso),     drvHead(_drvHead),
               commData(_commData), eyesRefGen(_eyesRefGen), loc(_loc),
               ctrl(_ctrl),         period(_period),         Ts(_period/1000.0)
{
    // Instantiate objects
    neck=new iCubHeadCenter(commData->head_version>1.0?"right_v2":"right");
    eyeL=new iCubEye(commData->head_version>1.0?"left_v2":"left");
    eyeR=new iCubEye(commData->head_version>1.0?"right_v2":"right");
    imu=new iCubInertialSensor(commData->head_version>1.0?"v2":"v1");

    // remove constraints on the links: logging purpose
    imu->setAllConstraints(false);

    // block neck dofs
    eyeL->blockLink(3,0.0); eyeR->blockLink(3,0.0);
    eyeL->blockLink(4,0.0); eyeR->blockLink(4,0.0);
    eyeL->blockLink(5,0.0); eyeR->blockLink(5,0.0);

    // Get the chain objects
    chainNeck=neck->asChain();
    chainEyeL=eyeL->asChain();        
    chainEyeR=eyeR->asChain();    

    // add aligning matrices read from configuration file
    getAlignHN(commData->rf_cameras,"ALIGN_KIN_LEFT",eyeL->asChain());
    getAlignHN(commData->rf_cameras,"ALIGN_KIN_RIGHT",eyeR->asChain());

    // overwrite aligning matrices iff specified through tweak values
    if (commData->tweakOverwrite)
    {
        getAlignHN(commData->rf_tweak,"ALIGN_KIN_LEFT",eyeL->asChain());
        getAlignHN(commData->rf_tweak,"ALIGN_KIN_RIGHT",eyeR->asChain());
    }

    // read number of joints
    if (drvTorso!=NULL)
    {
        IEncoders *encTorso; drvTorso->view(encTorso);
        encTorso->getAxes(&nJointsTorso);
    }
    else
        nJointsTorso=3;

    IEncoders *encHead; drvHead->view(encHead);
    encHead->getAxes(&nJointsHead);

    // joints bounds alignment
    alignJointsBounds(chainNeck,drvTorso,drvHead,commData->eyeTiltLim);

    // read starting position
    fbTorso.resize(nJointsTorso,0.0);
    fbHead.resize(nJointsHead,0.0);
    getFeedback(fbTorso,fbHead,drvTorso,drvHead,commData);

    copyJointsBounds(chainNeck,chainEyeL);
    copyJointsBounds(chainEyeL,chainEyeR);

    // store neck pitch/roll/yaw bounds
    neckPitchMin=(*chainNeck)[3].getMin();
    neckPitchMax=(*chainNeck)[3].getMax();
    neckRollMin =(*chainNeck)[4].getMin();
    neckRollMax =(*chainNeck)[4].getMax();
    neckYawMin  =(*chainNeck)[5].getMin();
    neckYawMax  =(*chainNeck)[5].getMax();

    neckPos.resize(3);
    gazePos.resize(3);
    updateAngles();

    updateTorsoBlockedJoints(chainNeck,fbTorso);
    chainNeck->setAng(3,fbHead[0]);
    chainNeck->setAng(4,fbHead[1]);
    chainNeck->setAng(5,fbHead[2]);

    updateTorsoBlockedJoints(chainEyeL,fbTorso);
    updateTorsoBlockedJoints(chainEyeR,fbTorso);
    updateNeckBlockedJoints(chainEyeL,fbHead);
    updateNeckBlockedJoints(chainEyeR,fbHead);

    Vector eyePos(2);
    eyePos[0]=gazePos[0];
    eyePos[1]=gazePos[1]+gazePos[2]/2.0;
    chainEyeL->setAng(eyePos);
    eyePos[1]=gazePos[1]-gazePos[2]/2.0;
    chainEyeR->setAng(eyePos);

    fbTorsoOld=fbTorso;
    neckAngleUserTolerance=0.0;
}

EyePinvRefGen::EyePinvRefGen(PolyDriver *_drvTorso, PolyDriver *_drvHead,
                             ExchangeData *_commData, Controller *_ctrl,
                             const Vector &_counterRotGain, const unsigned int _period) :
                             RateThread(_period), drvTorso(_drvTorso), drvHead(_drvHead),
                             commData(_commData), ctrl(_ctrl),         period(_period),
                             Ts(_period/1000.0),  counterRotGain(_counterRotGain)
{
    // Instantiate objects
    neck=new iCubHeadCenter(commData->head_version>1.0?"right_v2":"right");
    eyeL=new iCubEye(commData->head_version>1.0?"left_v2":"left");
    eyeR=new iCubEye(commData->head_version>1.0?"right_v2":"right");
    imu=new iCubInertialSensor(commData->head_version>1.0?"v2":"v1");

    // remove constraints on the links: logging purpose
    imu->setAllConstraints(false);

    // block neck dofs
    eyeL->blockLink(3,0.0); eyeR->blockLink(3,0.0);
    eyeL->blockLink(4,0.0); eyeR->blockLink(4,0.0);
    eyeL->blockLink(5,0.0); eyeR->blockLink(5,0.0);

    // Get the chain objects
    chainNeck=neck->asChain();
    chainEyeL=eyeL->asChain();
    chainEyeR=eyeR->asChain();

    // add aligning matrices read from configuration file
    getAlignHN(commData->rf_cameras,"ALIGN_KIN_LEFT",eyeL->asChain());
    getAlignHN(commData->rf_cameras,"ALIGN_KIN_RIGHT",eyeR->asChain());

    // overwrite aligning matrices iff specified through tweak values
    if (commData->tweakOverwrite)
    {
        getAlignHN(commData->rf_tweak,"ALIGN_KIN_LEFT",eyeL->asChain());
        getAlignHN(commData->rf_tweak,"ALIGN_KIN_RIGHT",eyeR->asChain());
    }

    if (commData->tweakOverwrite)
    {
        getAlignHN(commData->rf_tweak,"ALIGN_KIN_LEFT",eyeL->asChain());
        getAlignHN(commData->rf_tweak,"ALIGN_KIN_RIGHT",eyeR->asChain());
    }

    // get the length of the half of the eyes baseline
    eyesHalfBaseline=0.5*norm(eyeL->EndEffPose().subVector(0,2)-eyeR->EndEffPose().subVector(0,2));
    
    // read number of joints
    if (drvTorso!=NULL)
    {
        IEncoders *encTorso; drvTorso->view(encTorso);
        encTorso->getAxes(&nJointsTorso);
    }
    else
        nJointsTorso=3;

    IEncoders *encHead; drvHead->view(encHead);
    encHead->getAxes(&nJointsHead);

    // joints bounds alignment
    lim=alignJointsBounds(chainNeck,drvTorso,drvHead,commData->eyeTiltLim);
    copyJointsBounds(chainNeck,chainEyeL);
    copyJointsBounds(chainEyeL,chainEyeR);

    // just eye part is required
    lim=lim.submatrix(3,5,0,1);

    // reinforce vergence min bound
    lim(2,0)=commData->minAllowedVergence;

    // read starting position
    fbTorso.resize(nJointsTorso,0.0);
    fbHead.resize(nJointsHead,0.0);
    getFeedback(fbTorso,fbHead,drvTorso,drvHead,commData);

    // save original eyes tilt and pan bounds
    orig_eye_tilt_min=(*chainEyeL)[nJointsTorso+3].getMin();
    orig_eye_tilt_max=(*chainEyeL)[nJointsTorso+3].getMax();
    orig_eye_pan_min=(*chainEyeL)[nJointsTorso+4].getMin();
    orig_eye_pan_max=(*chainEyeL)[nJointsTorso+4].getMax();
    orig_lim=lim;

    // Instantiate integrator
    qd.resize(3);
    qd[0]=fbHead[3];
    qd[1]=fbHead[4];
    qd[2]=fbHead[5];
    I=new Integrator(Ts,qd,lim);

    fp.resize(3,0.0);
    eyesJ.resize(3,3);
    eyesJ.zero();

    genOn=false;
    saccadeUnderWayOld=false;
}

int main(int argc, char *argv[]) 
{
    // just list the devices if no argument given
    if (argc <= 2) {
        printf("You can call %s like this:\n", argv[0]);
        printf("   %s --robot ROBOTNAME --OPTION VALUE ...\n", argv[0]);
        printf("For example:\n");
        printf("   %s --robot icub --part any --remote /controlboard\n", argv[0]);
        printf("Here are devices listed for your system:\n");
        printf("%s", Drivers::factory().toString().c_str());
        return 0;
    }

    // get command line options
    Property options;
    options.fromCommand(argc, argv);
    if (!options.check("robot") || !options.check("part")) {
        printf("Missing either --robot or --part options\n");
        return 0;
    }

    Network yarp;
	Time::turboBoost();
    
    char name[1024];
    Value& v = options.find("robot");
    Value& part = options.find("part");

    Value *val;
    if (!options.check("device", val)) {
        options.put("device", "remote_controlboard");
    }
    if (!options.check("local", val)) {
        sprintf(name, "/%s/%s/client", v.asString().c_str(), part.asString().c_str());
        options.put("local", name);
    }
    if (!options.check("remote", val)) {
        sprintf(name, "/%s/%s", v.asString().c_str(), part.asString().c_str());
        options.put("remote", name);
    }

	fprintf(stderr, "%s", options.toString().c_str());

    
    // create a device 
    PolyDriver dd(options);
    if (!dd.isValid()) {
        printf("Device not available.  Here are the known devices:\n");
        printf("%s", Drivers::factory().toString().c_str());
        return 1;
    }

    IPositionControl *pos;
    IVelocityControl *vel;
    IEncoders *enc;
    IPidControl *pid;
    IAmplifierControl *amp;
    IControlLimits *lim;

    bool ok;
    ok = dd.view(pos);
    ok &= dd.view(vel);
    ok &= dd.view(enc);
    ok &= dd.view(pid);
    ok &= dd.view(amp);
    ok &= dd.view(lim);

    if (!ok) {
        printf("Problems acquiring interfaces\n");
        return 1;
    }

    int jnts = 0;
    pos->getAxes(&jnts);
    printf("Working with %d axes\n", jnts);
    double *tmp = new double[jnts];
    assert (tmp != NULL);

    printf("Device active...\n");
    while (dd.isValid()) {
        char s[1024];
        
        printf("-> ");
        char c = 0;
        int i = 0;
        while (c != '\n') {
            c = (char)fgetc(stdin);
            s[i++] = c;
        }
        s[i-1] = s[i] = 0;

        Bottle p;
        p.fromString(s);
        printf("Bottle: %s\n", p.toString().c_str());

        switch(p.get(0).asVocab()) {        
        case VOCAB_HELP:
            printf("\n\n");
            printf("Available commands:\n\n");

//.........这里部分代码省略.........

//.........这里部分代码省略.........
                        {
                            yInfo("Initializing a getTemps thread\n");
                            myDumper[i].setDevice(&ddBoard, &ddDebug, rate, portPrefix, dataToDump[i], logToFile);
                            myDumper[i].setThetaMap(thetaMap, nJoints);
                            myGetTemps.setInterface(imot);
                            if (ddBoard.view(istmp))
                            {
                                yInfo("getEncoders::The time stamp initalization interfaces was successfull! \n");
                                myGetTemps.setStamp(istmp);
                            }
                            else
                                yError("Problems getting the time stamp interfaces \n");
                            myDumper[i].setGetter(&myGetTemps);
                        }
                }
                else if (dataToDump[i] == "getMotorEncoders")
                {
                    if (ddBoard.view(imotenc))
                        {
                            yInfo("Initializing a getEncs thread\n");
                            myDumper[i].setDevice(&ddBoard, &ddDebug, rate, portPrefix, dataToDump[i], logToFile);
                            myDumper[i].setThetaMap(thetaMap, nJoints);
                            myGetMotorEncs.setInterface(imotenc);
                            if (ddBoard.view(istmp))
                            {
                                yInfo("getEncoders::The time stamp initalization interfaces was successfull! \n");
                                myGetMotorEncs.setStamp(istmp);
                            }
                            else
                                yError("Problems getting the time stamp interfaces \n");
                            myDumper[i].setGetter(&myGetMotorEncs);
                        }
                }
                else if (dataToDump[i] == "getMotorEncoderSpeeds")
                {
                    if (ddBoard.view(imotenc))
                        {
                            yInfo("Initializing a getSpeeds thread\n");
                            myDumper[i].setDevice(&ddBoard, &ddDebug, rate, portPrefix, dataToDump[i], logToFile);
                            myDumper[i].setThetaMap(thetaMap, nJoints);
                            myGetMotorSpeeds.setInterface(imotenc);
                            if (ddBoard.view(istmp))
                            {
                                yInfo("getEncodersSpeed::The time stamp initalization interfaces was successfull! \n");
                                myGetMotorSpeeds.setStamp(istmp);
                            }
                            else
                                yError("Problems getting the time stamp interfaces \n");
                            myDumper[i].setGetter(&myGetMotorSpeeds);
                        }
                }
                else if (dataToDump[i] == "getMotorEncoderAccelerations")
                {
                    if (ddBoard.view(imotenc))
                        {
                            yInfo("Initializing a getAccs thread\n");
                            myDumper[i].setDevice(&ddBoard, &ddDebug, rate, portPrefix, dataToDump[i], logToFile);
                            myDumper[i].setThetaMap(thetaMap, nJoints);
                            myGetMotorAccs.setInterface(imotenc);
                            if (ddBoard.view(istmp))
                            {
                                yInfo("getEncoderAccelerations::The time stamp initalization interfaces was successfull! \n");
                                myGetMotorAccs.setStamp(istmp);
                            }
                            else
                                yError("Problems getting the time stamp interfaces \n");
                            myDumper[i].setGetter(&myGetMotorAccs);
                        }
                }
                else if (dataToDump[i] == "getMotorsPwm")
                 {
                     if (ddBoard.view(iamp))
                        {
                            yInfo("Initializing a getMotPwm thread\n");
                            myDumper[i].setDevice(&ddBoard, &ddDebug, rate, portPrefix, dataToDump[i], logToFile);
                            myDumper[i].setThetaMap(thetaMap, nJoints);
                            myGetMotPwm.setInterface(iamp);
                            if(ienc == 0)
                            {
                                if(!ddBoard.view(ienc))
                                    return false;
                            }
                            ienc->getAxes(&myGetMotPwm.n_joint_part);
                            if (ddBoard.view(istmp))
                            {
                                yInfo("getMotorsPwm::The time stamp initalization interfaces was successfull! \n");
                                myGetMotPwm.setStamp(istmp);
                            }
                            else
                                yError("Problems getting the time stamp interfaces \n");
                            myDumper[i].setGetter(&myGetMotPwm);
                        }
                }
            }
        Time::delay(1);
        for (int i = 0; i < nData; i++)
            myDumper[i].start();

        return true;
    }