C++ IEncoders类代码示例

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

void partMover::run_click(GtkButton *button, gtkClassData* currentClassData)
{
  partMover *currentPart = currentClassData->partPointer;
  int * joint = currentClassData->indexPointer;
  IPositionControl *ipos = currentPart->pos;
  IEncoders *iiencs = currentPart->iencs;
  IAmplifierControl *iamp = currentPart->amp;
  IPidControl *ipid = currentPart->pid;
  GtkWidget **sliderAry = currentPart->sliderArray;
	 
  double posJoint;
	
  while (!iiencs->getEncoder(*joint, &posJoint))
    Time::delay(0.001);
  iamp->enableAmp(*joint);
  ipid->enablePid(*joint);
  gtk_range_set_value ((GtkRange *) (sliderAry[*joint]), posJoint);
  return;
}

void partMover::idle_all(GtkButton *button, partMover* currentPart)
{
  IPositionControl *ipos = currentPart->pos;
  IEncoders *iiencs = currentPart->iencs;
  IAmplifierControl *iamp = currentPart->amp;
  IPidControl *ipid = currentPart->pid;
  GtkWidget **sliderAry = currentPart->sliderArray;
	 
  double posJoint;
  int joint;
  int NUMBER_OF_JOINTS;
  ipos->getAxes(&NUMBER_OF_JOINTS);
  
  for (joint=0; joint < NUMBER_OF_JOINTS; joint++)
  {
    iiencs->getEncoder(joint, &posJoint);
    iamp->disableAmp(joint);
    ipid->disablePid(joint);
    gtk_range_set_value ((GtkRange *) (sliderAry[joint]), posJoint);
  }
  return;
}

bool partMover::entry_update(partMover *currentPart)
{
  GdkColor color_black;
  GdkColor color_grey;
  GdkColor color_yellow;
  GdkColor color_green;
  GdkColor color_green_blue;
  GdkColor color_dark_green;
  GdkColor color_red;
  GdkColor color_fault_red;
  GdkColor color_pink;
  GdkColor color_indaco;
  GdkColor color_white;
  GdkColor color_blue;
  
  color_pink.red=219*255;
  color_pink.green=166*255;
  color_pink.blue=171*255;

  color_fault_red.red=255*255;
  color_fault_red.green=10*255;
  color_fault_red.blue=10*255;

  color_black.red=10*255;
  color_black.green=10*255;
  color_black.blue=10*255;

  color_red.red=255*255;
  color_red.green=100*255;
  color_red.blue=100*255;

  color_grey.red=220*255;
  color_grey.green=220*255;
  color_grey.blue=220*255;

  color_white.red=250*255;
  color_white.green=250*255;
  color_white.blue=250*255;

  color_green.red=149*255;
  color_green.green=221*255;
  color_green.blue=186*255;

  color_dark_green.red=(149-30)*255;
  color_dark_green.green=(221-30)*255;
  color_dark_green.blue=(186-30)*255;

  color_blue.red=150*255;
  color_blue.green=190*255;
  color_blue.blue=255*255;

  color_green_blue.red=(149+150)/2*255;
  color_green_blue.green=(221+190)/2*255;
  color_green_blue.blue=(186+255)/2*255;

  color_indaco.red=220*255;
  color_indaco.green=190*255;
  color_indaco.blue=220*255;

  color_yellow.red=249*255;
  color_yellow.green=236*255;
  color_yellow.blue=141*255;
  
  GdkColor* pColor= &color_grey;

  static int slowSwitcher = 0;

  IControlMode     *ictrl = currentPart->ctrlmode2;
  IInteractionMode *iint  = currentPart->iinteract;
  IPositionControl  *ipos = currentPart->pos;
  IVelocityControl  *ivel = currentPart->iVel;
  IPositionDirect   *iDir = currentPart->iDir;
  IEncoders       *iiencs = currentPart->iencs;
  ITorqueControl    *itrq = currentPart->trq;
  IAmplifierControl *iamp = currentPart->amp;

  GtkEntry * *pos_entry   = (GtkEntry **)  currentPart->currPosArray;
  GtkEntry  **trq_entry   = (GtkEntry **)  currentPart->currTrqArray;
  GtkEntry  **speed_entry = (GtkEntry **)  currentPart->currSpeedArray;
  GtkEntry    **inEntry   = (GtkEntry **)  currentPart->inPosArray;
  GtkWidget **colorback   = (GtkWidget **) currentPart->frameColorBack;

  GtkWidget **sliderAry = currentPart->sliderArray;
  bool *POS_UPDATE = currentPart->CURRENT_POS_UPDATE;

  char buffer[40] = {'i', 'n', 'i', 't'};
  char frame_title [255];

  double positions[MAX_NUMBER_OF_JOINTS];
  double torques[MAX_NUMBER_OF_JOINTS];
  double speeds[MAX_NUMBER_OF_JOINTS];
  double max_torques[MAX_NUMBER_OF_JOINTS];
  double min_torques[MAX_NUMBER_OF_JOINTS];
  static int controlModes[MAX_NUMBER_OF_JOINTS];
  static int controlModesOld[MAX_NUMBER_OF_JOINTS];
  static yarp::dev::InteractionModeEnum interactionModes[MAX_NUMBER_OF_JOINTS];
  static yarp::dev::InteractionModeEnum interactionModesOld[MAX_NUMBER_OF_JOINTS];

  int k;
  int NUMBER_OF_JOINTS=0;
//.........这里部分代码省略.........

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);
//.........这里部分代码省略.........

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 --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;
//.........这里部分代码省略.........

//.........这里部分代码省略.........
        {
            yInfo("elapsed %d [s]",(int)(Time::now()-t0));
            Time::delay(1.0);
            if (interrupting)
            {
                idlingCoupledJoints(i,false);
                return false;
            }
        }

        Property pResults;
        designer.getResults(pResults);
        double tau=pResults.find("tau_mean").asDouble();
        double K=pResults.find("K_mean").asDouble();
        yInfo("plant = %g/s * 1/(1+s*%g)",K,tau);

        Property pControllerRequirements,pController;
        pControllerRequirements.put("tau",tau);
        pControllerRequirements.put("K",K);
        pControllerRequirements.put("f_c",0.75);

        if (i!=15)
        {
            pControllerRequirements.put("T_dr",1.0);
            pControllerRequirements.put("type","PI");
        }
        else
            pControllerRequirements.put("type","P");

        designer.tuneController(pControllerRequirements,pController);
        yInfo("tuning results: %s",pController.toString().c_str());
        double Kp=pController.find("Kp").asDouble();
        double Ki=pController.find("Ki").asDouble();
        pid.scale=4.0;
        int scale=(int)pid.scale; int shift=1<<scale;
        double fwKp=floor(Kp*pid.encs_ratio*shift);
        double fwKi=floor(Ki*pid.encs_ratio*shift/1000.0);
        pid.Kp=yarp::math::sign(pid.Kp*fwKp)>0.0?fwKp:-fwKp;
        pid.Ki=yarp::math::sign(pid.Ki*fwKi)>0.0?fwKi:-fwKi;
        pid.Kd=0.0;
        yInfo("Kp (FW) = %g; Ki (FW) = %g; Kd (FW) = %g; shift factor = %d",pid.Kp,pid.Ki,pid.Kd,scale);

        Property pStictionEstimation;
        pStictionEstimation.put("max_time",60.0);
        pStictionEstimation.put("Kp",Kp);
        pStictionEstimation.put("Ki",0.0);
        pStictionEstimation.put("Kd",0.0);
        designer.startStictionEstimation(pStictionEstimation);

        yInfo("Estimating stiction for joint %d: max duration = %g seconds",
              i,pStictionEstimation.find("max_time").asDouble());

        t0=Time::now();
        while (!designer.isDone())
        {
            yInfo("elapsed %d [s]",(int)(Time::now()-t0));
            Time::delay(1.0);
            if (interrupting)
            {
                idlingCoupledJoints(i,false);
                return false;
            }
        }

        designer.getResults(pResults);
        pid.st_up=floor(pResults.find("stiction").asList()->get(0).asDouble());
        pid.st_down=floor(pResults.find("stiction").asList()->get(1).asDouble());
        yInfo("Stiction values: up = %g; down = %g",pid.st_up,pid.st_down);

        IControlMode2 *imod;
        IPositionControl *ipos;
        IEncoders *ienc;
        driver->view(imod);
        driver->view(ipos);
        driver->view(ienc);
        imod->setControlMode(i,VOCAB_CM_POSITION);
        ipos->setRefSpeed(i,50.0);
        ipos->positionMove(i,0.0);
        yInfo("Driving the joint back to rest... ");
        t0=Time::now();
        while (Time::now()-t0<5.0)
        {
            double enc;
            ienc->getEncoder(i,&enc);
            if (fabs(enc)<1.0)
                break;

            if (interrupting)
            {
                idlingCoupledJoints(i,false);
                return false;
            }

            Time::delay(0.2);
        }
        yInfo("done!");

        idlingCoupledJoints(i,false);
        return true;
    }

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());
//.........这里部分代码省略.........

void partMover::fixed_time_move(const double *cmdPositions, double cmdTime, partMover* currentPart)
{
  IPositionControl *ipos = currentPart->pos;
  IEncoders *iiencs = currentPart->iencs;
  IAmplifierControl *iamp = currentPart->amp;
  IPidControl *ipid = currentPart->pid;
  ITorqueControl *itrq= currentPart->trq;

  int *SEQUENCE_TMP = currentPart->SEQUENCE;
  double *TIMING_TMP = currentPart->TIMING;
  double **STORED_POS_TMP = currentPart->STORED_POS;
  double **STORED_VEL_TMP = currentPart->STORED_VEL;
  GtkWidget **sliderAry = currentPart->sliderArray;
  GtkWidget **sliderVelAry = currentPart->sliderVelArray;
  int NUM_JOINTS;
  ipos->getAxes(&NUM_JOINTS);
  double *cmdVelocities = new double[NUM_JOINTS];
  double *startPositions = new double[NUM_JOINTS];

  while (!iiencs->getEncoders(startPositions))
    Time::delay(0.001);
    //fprintf(stderr, "getEncoders is returning false\n");
    //fprintf(stderr, "Getting the following values for the encoders");
  //for(int k=0; k<NUM_JOINTS; k++)
  //  fprintf(stderr, "%.1f ", startPositions[k]);
  //fprintf(stderr, "\n");  

  int k;
  for(k=0; k<NUM_JOINTS; k++)
    {
      cmdVelocities[k] = 0;
		
      if (fabs(startPositions[k] - cmdPositions[k]) > 0.01)
	cmdVelocities[k] = fabs(startPositions[k] - cmdPositions[k])/cmdTime;
      else
	cmdVelocities[k] = 1.0;
    }

  //fprintf(stderr, "ExplorerThread-> Start pos:\n");
  //for(int j=0; j < NUM_JOINTS; j++)
  //  fprintf(stderr, "%.2lf\t", startPositions[j]);
  //fprintf(stderr, "\n");
  //fprintf(stderr, "ExplorerThread-> Moving arm to:\n");
  //for(int j=0; j < NUM_JOINTS; j++)
  //  fprintf(stderr, "%.2lf\t", cmdPositions[j]);
  //fprintf(stderr, "\n");
  //fprintf(stderr, "ExplorerThread-> with velocity:\n");
  //for(int ii=0; ii < NUM_JOINTS; ii++)
  //  fprintf(stderr, "%.2lf\t", cmdVelocities[ii]);
  //fprintf(stderr, "\n");

  ipos->setRefSpeeds(cmdVelocities);
  ipos->positionMove(cmdPositions);	

  currentPart->sequence_port_stamp.update();
  currentPart->sequence_port.setEnvelope(currentPart->sequence_port_stamp);
  Vector v(NUM_JOINTS,cmdPositions);
  currentPart->sequence_port.write(v);
  delete cmdVelocities;
  delete startPositions;
  return;
}

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");

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

    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 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 run()
    {
        //do the work
        iencs->getEncoders(encoders.data());

        count++;

        if (count%2)
            commands=5;
        else
            commands=-5;
    
        ivel->velocityMove(commands.data());

        printf(".");
    }

    bool updateModule()
    {
        if (calibrate)
        {
            Property options;
            options.put("finger",fingerName.c_str());
            model->calibrate(options);
            calibrate=false;

            ipos->setRefAcceleration(joint,1e9);
            if ((fingerName=="ring")||(fingerName=="little"))
                ipos->setRefSpeed(joint,60.0);
            else
                ipos->setRefSpeed(joint,30.0);

            ipos->positionMove(joint,*val);
        }
        else
        {
            if (Node *finger=model->getNode(fingerName))
            {
                Value data; finger->getSensorsData(data);
                Value out;  finger->getOutput(out);
                fprintf(stdout,"%s sensors data = %s; output = %s\n",
                        finger->getName().c_str(),data.toString().c_str(),out.toString().c_str());
            }
            
            double fb; ienc->getEncoder(joint,&fb);
            if (fabs(*val-fb)<5.0)
            {
                val==&min?val=&max:val=&min;
                ipos->positionMove(joint,*val);
            }
        }

        return true;
    }