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 ****************** imagePortInR -> open(("/"+name+"/imageR:i").c_str()); imagePortInL -> open(("/"+name+"/imageL:i").c_str()); imagePortOutR.open(("/"+name+"/imageR:o").c_str()); imagePortOutL.open(("/"+name+"/imageL:o").c_str()); portOutInfo.open(("/"+name+"/info:o").c_str()); if (robot=="icub") { Network::connect("/icub/camcalib/left/out",("/"+name+"/imageL:i").c_str()); Network::connect("/icub/camcalib/right/out",("/"+name+"/imageR:i").c_str()); } else { Network::connect("/icubSim/cam/left",("/"+name+"/imageL:i").c_str()); Network::connect("/icubSim/cam/right",("/"+name+"/imageR:i").c_str()); } Network::connect(("/"+name+"/imageL:o").c_str(),"/demoINN/left"); Network::connect(("/"+name+"/imageR:o").c_str(),"/demoINN/right"); Network::connect(("/"+name+"/info:o").c_str(),"/iSpeak"); rpcClnt.open(("/"+name+"/rpc:o").c_str()); rpcSrvr.open(("/"+name+"/rpc:i").c_str()); attach(rpcSrvr); printMessage(0,"Configure Finished!\n"); return true; }
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); eyesJoints.resize(3); neckJoints[0]=0; neckJoints[1]=1; neckJoints[2]=2; eyesJoints[0]=3; eyesJoints[1]=4; eyesJoints[2]=5; qd=fbHead; q0deg=CTRL_RAD2DEG*qd; qddeg=q0deg; vdeg =CTRL_RAD2DEG*v; port_xd=NULL; ctrlActiveRisingEdgeTime=0.0; saccadeStartTime=0.0; unplugCtrlEyes=false; ctrlInhibited=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; int i = 0; while (c != '\n') { c = (char)fgetc(stdin); s[i++] = c; } s[i-1] = s[i] = 0; Bottle p; Bottle response; bool ok=false; bool rec=false; p.fromString(s.c_str()); printf("Bottle: %s\n", p.toString().c_str()); switch(p.get(0).asVocab()) { case VOCAB_HELP: printf("\n\n"); printf("Available commands:\n"); printf("-------------------\n\n"); printf("IOpenLoop:\ntype [%s] and one of the following:\n", Vocab::decode(VOCAB_IOPENLOOP).c_str()); printf(" [set] [%s] <int> <float>\n", Vocab::decode(VOCAB_OUTPUT).c_str()); printf(" [get] [%s] <int>\n", Vocab::decode(VOCAB_OUTPUT).c_str()); printf(" [get] [%s]\n\n", Vocab::decode(VOCAB_OUTPUTS).c_str()); printf("IControlMode:\ntype [%s] and one of the following:\n", Vocab::decode(VOCAB_ICONTROLMODE).c_str()); printf(" [set] [%s]|[%s]|[%s]|[%s]|[%s]|[%s]|[%s]|[%s][%s]|[%s]\n", Vocab::decode(VOCAB_CM_POSITION).c_str(), Vocab::decode(VOCAB_CM_POSITION_DIRECT).c_str(), Vocab::decode(VOCAB_CM_VELOCITY).c_str(), Vocab::decode(VOCAB_CM_MIXED).c_str(), Vocab::decode(VOCAB_CM_TORQUE).c_str(), Vocab::decode(VOCAB_CM_OPENLOOP).c_str(), Vocab::decode(VOCAB_CM_IDLE).c_str(), Vocab::decode(VOCAB_CM_FORCE_IDLE).c_str(), Vocab::decode(VOCAB_CM_IMPEDANCE_POS).c_str(), Vocab::decode(VOCAB_CM_IMPEDANCE_VEL).c_str()); printf(" [get] [%s] <int>\n\n", Vocab::decode(VOCAB_CM_CONTROL_MODE).c_str()); printf("ITorqueControl:\ntype [%s] and one of the following:\n", Vocab::decode(VOCAB_TORQUE).c_str()); printf(" [get] [%s] <int> to read the measured torque for a single axis\n", Vocab::decode(VOCAB_TRQ).c_str()); printf(" [get] [%s] to read the measured torque for all axes\n", Vocab::decode(VOCAB_TRQS).c_str()); printf(" [set] [%s] <int> <float> to set the reference torque for a single axis\n", Vocab::decode(VOCAB_REF).c_str()); printf(" [set] [%s] <float list> to set the reference torque for all axes\n", Vocab::decode(VOCAB_REFS).c_str()); printf(" [get] [%s] <int> to read the reference torque for a single axis\n", Vocab::decode(VOCAB_REF).c_str()); printf(" [get] [%s] to read the reference torque for all axes\n\n", Vocab::decode(VOCAB_REFS).c_str()); printf("IImpedanceControl:\ntype [%s] and one of the following:\n", Vocab::decode(VOCAB_IMPEDANCE).c_str()); printf(" [set] [%s] <int> <float> <float> \n", Vocab::decode(VOCAB_IMP_PARAM).c_str()); printf(" [set] [%s] <int> <float>\n\n", Vocab::decode(VOCAB_IMP_OFFSET).c_str()); printf(" [get] [%s] <int>\n", Vocab::decode(VOCAB_IMP_PARAM).c_str()); printf(" [get] [%s] <int>\n\n", Vocab::decode(VOCAB_IMP_OFFSET).c_str()); printf("IInteractionMode:\ntype [%s] and one of the following:\n", Vocab::decode(VOCAB_INTERFACE_INTERACTION_MODE).c_str()); printf(" [set] [%s]|[%s] <int>\n", Vocab::decode(VOCAB_IM_STIFF).c_str(), Vocab::decode(VOCAB_IM_COMPLIANT).c_str()); printf(" [get] [%s] <int>\n", Vocab::decode(VOCAB_INTERACTION_MODE).c_str()); printf(" [get] [%s] \n\n", Vocab::decode(VOCAB_INTERACTION_MODES).c_str()); printf("Standard Interfaces:\n"); printf("type [get] and one of the following:\n"); printf(" [%s] to read the number of controlled axes\n", Vocab::decode(VOCAB_AXES).c_str()); printf(" [%s] to read the encoder value for all axes\n", Vocab::decode(VOCAB_ENCODERS).c_str()); printf(" [%s] to read the PID values for all axes\n", Vocab::decode(VOCAB_PIDS).c_str()); printf(" [%s] <int> to read the PID values for a single axis\n", Vocab::decode(VOCAB_PID).c_str()); printf(" [%s] <int> to read the limit values for a single axis\n", Vocab::decode(VOCAB_LIMITS).c_str()); printf(" [%s] to read the PID error for all axes\n", Vocab::decode(VOCAB_ERRS).c_str()); printf(" [%s] to read the PID output for all axes\n", Vocab::decode(VOCAB_OUTPUTS).c_str()); printf(" [%s] to read the reference position for all axes\n", Vocab::decode(VOCAB_REFERENCES).c_str()); printf(" [%s] <int> to read the reference position for a single axis\n", Vocab::decode(VOCAB_REFERENCE).c_str()); printf(" [%s] to read the reference speed for all axes\n", Vocab::decode(VOCAB_REF_SPEEDS).c_str()); printf(" [%s] <int> to read the reference speed for a single axis\n", Vocab::decode(VOCAB_REF_SPEED).c_str()); printf(" [%s] to read the reference acceleration for all axes\n", Vocab::decode(VOCAB_REF_ACCELERATIONS).c_str()); printf(" [%s] <int> to read the reference acceleration for a single axis\n", Vocab::decode(VOCAB_REF_ACCELERATION).c_str()); printf(" [%s] to read the current consumption for all axes\n", Vocab::decode(VOCAB_AMP_CURRENTS).c_str()); printf("\n"); printf("type [set] and one of the following:\n"); printf(" [%s] <int> <double> to move a single axis\n", Vocab::decode(VOCAB_POSITION_MOVE).c_str()); printf(" [%s] <int> <double> to accelerate a single axis to a given speed\n", Vocab::decode(VOCAB_VELOCITY_MOVE).c_str()); printf(" [%s] <int> <double> to set the reference speed for a single axis\n", Vocab::decode(VOCAB_REF_SPEED).c_str()); printf(" [%s] <int> <double> to set the reference acceleration for a single axis\n", Vocab::decode(VOCAB_REF_ACCELERATION).c_str()); printf(" [%s] <list> to move multiple axes\n", Vocab::decode(VOCAB_POSITION_MOVES).c_str()); printf(" [%s] <list> to accelerate multiple axes to a given speed\n", Vocab::decode(VOCAB_VELOCITY_MOVES).c_str()); printf(" [%s] <list> to set the reference speed for all axes\n", Vocab::decode(VOCAB_REF_SPEEDS).c_str()); printf(" [%s] <list> to set the reference acceleration for all axes\n", Vocab::decode(VOCAB_REF_ACCELERATIONS).c_str()); printf(" [%s] <int> to stop a single axis\n", Vocab::decode(VOCAB_STOP).c_str()); printf(" [%s] <int> to stop all axes\n", Vocab::decode(VOCAB_STOPS).c_str()); printf(" [%s] <int> <list> to set the PID values for a single axis\n", Vocab::decode(VOCAB_PID).c_str()); printf(" [%s] <int> <list> to set the limits for a single axis\n", Vocab::decode(VOCAB_LIMITS).c_str()); printf(" [%s] <int> to disable the PID control for a single axis\n", Vocab::decode(VOCAB_DISABLE).c_str()); printf(" [%s] <int> to enable the PID control for a single axis\n", Vocab::decode(VOCAB_ENABLE).c_str()); printf(" [%s] <int> <double> to set the encoder value for a single axis\n", Vocab::decode(VOCAB_ENCODER).c_str()); printf(" [%s] <list> to set the encoder value for all axes\n", Vocab::decode(VOCAB_ENCODERS).c_str()); printf("\n"); printf("NOTES: - A list is a sequence of numbers in parenthesis, e.g. (10 2 1 10)\n"); printf(" - Pids are expressed as a list of 7 numbers, type get pid <int> to see an example\n"); printf("\n"); break; case VOCAB_QUIT: goto ApplicationCleanQuit; break; case VOCAB_ICONTROLMODE: { handleControlModeMsg(iMode2, p, response, &rec, &ok); printf("%s\n", response.toString().c_str()); break; } case VOCAB_IMPEDANCE: { handleImpedanceMsg(iimp, p, response, &rec, &ok); printf("%s\n", response.toString().c_str()); break; } case VOCAB_TORQUE: { handleTorqueMsg(itorque, p, response, &rec, &ok); printf("%s\n", response.toString().c_str()); break; } case VOCAB_INTERFACE_INTERACTION_MODE: { handleInteractionModeMsg(iInteract, p, response, &rec, &ok); printf("%s\n", response.toString().c_str()); break; } case VOCAB_GET: switch(p.get(1).asVocab()) { case VOCAB_AXES: { int nj = 0; enc->getAxes(&nj); printf ("%s: %d\n", Vocab::decode(VOCAB_AXES).c_str(), nj); } break; case VOCAB_ENCODERS: { enc->getEncoders(tmp); printf ("%s: (", Vocab::decode(VOCAB_ENCODERS).c_str()); for(i = 0; i < jnts; i++) printf ("%.2f ", tmp[i]); printf (")\n"); } break; case VOCAB_PID: { Pid pd; int j = p.get(2).asInt(); pid->getPid(j, &pd); printf("%s: ", Vocab::decode(VOCAB_PID).c_str()); printf("kp %.2f ", pd.kp); printf("kd %.2f ", pd.kd); printf("ki %.2f ", pd.ki); printf("maxi %.2f ", pd.max_int); printf("maxo %.2f ", pd.max_output); printf("off %.2f ", pd.offset); printf("scale %.2f ", pd.scale); printf("\n"); } break; case VOCAB_PIDS: { Pid *p = new Pid[jnts]; ok = pid->getPids(p); Bottle& b = response.addList(); int i; for (i = 0; i < jnts; i++) { Bottle& c = b.addList(); c.addDouble(p[i].kp); c.addDouble(p[i].kd); c.addDouble(p[i].ki); c.addDouble(p[i].max_int); c.addDouble(p[i].max_output); c.addDouble(p[i].offset); c.addDouble(p[i].scale); } printf("%s\n", b.toString().c_str()); delete[] p; } break; case VOCAB_LIMITS: { double min, max; int j = p.get(2).asInt(); lim->getLimits(j, &min, &max); printf("%s: ", Vocab::decode(VOCAB_LIMITS).c_str()); printf("limits: (%.2f %.2f)\n", min, max); } break; case VOCAB_ERRS: { pid->getErrors(tmp); printf ("%s: (", Vocab::decode(VOCAB_ERRS).c_str()); for(i = 0; i < jnts; i++) printf ("%.2f ", tmp[i]); printf (")\n"); } break; case VOCAB_OUTPUTS: { iopenloop->getOutputs(tmp); printf ("%s: (", Vocab::decode(VOCAB_OUTPUTS).c_str()); for(i = 0; i < jnts; i++) printf ("%.2f ", tmp[i]); printf (")\n"); } break; case VOCAB_OUTPUT: { int j = p.get(2).asInt(); double v; iopenloop->getOutput(j, &v); printf("%s: ", Vocab::decode(VOCAB_OUTPUT).c_str()); printf("%.2f ", v); printf("\n"); } break; case VOCAB_REFERENCE: { double ref_pos; int j = p.get(2).asInt(); pid->getReference(j,&ref_pos); printf ("%s: (", Vocab::decode(VOCAB_REFERENCE).c_str()); printf ("%.2f ", ref_pos); printf (")\n"); } break; case VOCAB_REFERENCES: { pid->getReferences(tmp); printf ("%s: (", Vocab::decode(VOCAB_REFERENCES).c_str()); for(i = 0; i < jnts; i++) printf ("%.2f ", tmp[i]); printf (")\n"); } break; case VOCAB_REF_SPEEDS: { pos->getRefSpeeds(tmp); printf ("%s: (", Vocab::decode(VOCAB_REF_SPEEDS).c_str()); for(i = 0; i < jnts; i++) printf ("%.2f ", tmp[i]); printf (")\n"); } break; case VOCAB_REF_SPEED: { double ref_speed; int j = p.get(2).asInt(); pos->getRefSpeed(j,&ref_speed); printf ("%s: (", Vocab::decode(VOCAB_REF_SPEED).c_str()); printf ("%.2f ", ref_speed); printf (")\n"); } break; case VOCAB_REF_ACCELERATION: { double ref_acc; int j = p.get(2).asInt(); pos->getRefAcceleration(j,&ref_acc); printf ("%s: (", Vocab::decode(VOCAB_REF_ACCELERATION).c_str()); printf ("%.2f ", ref_acc); printf (")\n"); } break; case VOCAB_REF_ACCELERATIONS: { pos->getRefAccelerations(tmp); printf ("%s: (", Vocab::decode(VOCAB_REF_ACCELERATIONS).c_str()); for(i = 0; i < jnts; i++) printf ("%.2f ", tmp[i]); printf (")\n"); } break; case VOCAB_AMP_CURRENTS: { amp->getCurrents(tmp); printf ("%s: (", Vocab::decode(VOCAB_AMP_CURRENTS).c_str()); for(i = 0; i < jnts; i++) printf ("%.2f ", tmp[i]); printf (")\n"); } break; } break; case VOCAB_SET: switch(p.get(1).asVocab()) { case VOCAB_POSITION_MOVE: { int j = p.get(2).asInt(); double ref = p.get(3).asDouble(); printf("%s: moving %d to %.2f\n", Vocab::decode(VOCAB_POSITION_MOVE).c_str(), j, ref); pos->positionMove(j, ref); } break; case VOCAB_VELOCITY_MOVE: { int j = p.get(2).asInt(); double ref = p.get(3).asDouble(); printf("%s: accelerating %d to %.2f\n", Vocab::decode(VOCAB_VELOCITY_MOVE).c_str(), j, ref); vel->velocityMove(j, ref); } break; case VOCAB_REF_SPEED: { int j = p.get(2).asInt(); double ref = p.get(3).asDouble(); printf("%s: setting speed for %d to %.2f\n", Vocab::decode(VOCAB_REF_SPEED).c_str(), j, ref); pos->setRefSpeed(j, ref); } break; case VOCAB_REF_ACCELERATION: { int j = p.get(2).asInt(); double ref = p.get(3).asDouble(); printf("%s: setting acceleration for %d to %.2f\n", Vocab::decode(VOCAB_REF_ACCELERATION).c_str(), j, ref); pos->setRefAcceleration(j, ref); } break; case VOCAB_POSITION_MOVES: { Bottle *l = p.get(2).asList(); for (i = 0; i < jnts; i++) { tmp[i] = l->get(i).asDouble(); } printf("%s: moving all joints\n", Vocab::decode(VOCAB_POSITION_MOVES).c_str()); pos->positionMove(tmp); } break; case VOCAB_VELOCITY_MOVES: { Bottle *l = p.get(2).asList(); for (i = 0; i < jnts; i++) { tmp[i] = l->get(i).asDouble(); } printf("%s: moving all joints\n", Vocab::decode(VOCAB_VELOCITY_MOVES).c_str()); vel->velocityMove(tmp); } break; case VOCAB_REF_SPEEDS: { Bottle *l = p.get(2).asList(); for (i = 0; i < jnts; i++) { tmp[i] = l->get(i).asDouble(); } printf("%s: setting speed for all joints\n", Vocab::decode(VOCAB_REF_SPEEDS).c_str()); pos->setRefSpeeds(tmp); } break; case VOCAB_REF_ACCELERATIONS: { Bottle *l = p.get(2).asList(); for (i = 0; i < jnts; i++) { tmp[i] = l->get(i).asDouble(); } printf("%s: setting acceleration for all joints\n", Vocab::decode(VOCAB_REF_ACCELERATIONS).c_str()); pos->setRefAccelerations(tmp); } break; case VOCAB_STOP: { int j = p.get(2).asInt(); printf("%s: stopping axis %d\n", Vocab::decode(VOCAB_STOP).c_str(), j); pos->stop(j); } break; case VOCAB_STOPS: { printf("%s: stopping all axes\n", Vocab::decode(VOCAB_STOPS).c_str()); pos->stop(); } break; case VOCAB_ENCODER: { int j = p.get(2).asInt(); double ref = p.get(3).asDouble(); printf("%s: setting the encoder value for %d to %.2f\n", Vocab::decode(VOCAB_ENCODER).c_str(), j, ref); enc->setEncoder(j, ref); } break; case VOCAB_ENCODERS: { Bottle *l = p.get(2).asList(); for (i = 0; i < jnts; i++) { tmp[i] = l->get(i).asDouble(); } printf("%s: setting the encoder value for all joints\n", Vocab::decode(VOCAB_ENCODERS).c_str()); enc->setEncoders(tmp); } break; case VOCAB_PID: { Pid pd; int j = p.get(2).asInt(); Bottle *l = p.get(3).asList(); if (l==0) { printf("Check you specify a 7 elements list, e.g. set pid 0 (2000 20 1 300 300 0 0)\n"); } else { int elems=l->size(); if (elems>=3) { pd.kp = l->get(0).asDouble(); pd.kd = l->get(1).asDouble(); pd.ki = l->get(2).asDouble(); if (elems>=7) { pd.max_int = l->get(3).asDouble(); pd.max_output = l->get(4).asDouble(); pd.offset = l->get(5).asDouble(); pd.scale = l->get(6).asDouble(); } printf("%s: setting PID values for axis %d\n", Vocab::decode(VOCAB_PID).c_str(), j); pid->setPid(j, pd); } else { printf("Error, check you specify at least 7 elements, e.g. set pid 0 (2000 20 1 300 300 0 0)\n"); } } } break; case VOCAB_DISABLE: { int j = p.get(2).asInt(); printf("%s: disabling control for axis %d\n", Vocab::decode(VOCAB_DISABLE).c_str(), j); pid->disablePid(j); amp->disableAmp(j); } break; case VOCAB_ENABLE: { int j = p.get(2).asInt(); printf("%s: enabling control for axis %d\n", Vocab::decode(VOCAB_ENABLE).c_str(), j); amp->enableAmp(j); pid->enablePid(j); } break; case VOCAB_LIMITS: { int j = p.get(2).asInt(); printf("%s: setting limits for axis %d\n", Vocab::decode(VOCAB_LIMITS).c_str(), j); Bottle *l = p.get(3).asList(); lim->setLimits(j, l->get(0).asDouble(), l->get(1).asDouble()); } break; case VOCAB_OUTPUT: { int j=p.get(2).asInt(); double v=p.get(3).asDouble(); iopenloop->setRefOutput(j,v); printf("%s: setting output for axis %d to %f\n", Vocab::decode(VOCAB_OUTPUT).c_str(), j, v); } break; } break; } /* switch get(0) */ } /* while () */ ApplicationCleanQuit: dd.close(); delete[] tmp; Network::fini(); return 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; while((!done) || (time==-1)) { count++; double v; int jj=0; for(jj=0; jj<nj; jj++) { // ienc->getEncoder(jj, encoders.data()+jj); // iamp->enableAmp(jj); // ilim->setLimits(jj, 0, 0); // double max; // double min; // ilim->getLimits(jj, &min, &max); fprintf(stderr, "."); // Pid pid; // ipid->getPid(jj, &pid); bool done; ipos->checkMotionDone(jj, &done); fprintf(stderr, "#\n"); } fprintf(stderr, "%u\n", count); Time::delay(0.1); now=Time::now(); if ((now-startT)>time) done=true; } printf("bye bye from %d\n", id); return 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()); bool fingers_ok=true; fingers_ok&=fingers[0].fromProperty(thumb); fingers_ok&=fingers[1].fromProperty(index); fingers_ok&=fingers[2].fromProperty(middle); fingers_ok&=fingers[3].fromProperty(ring); fingers_ok&=fingers[4].fromProperty(little); if (!fingers_ok) { printMessage(1,"some errors occured\n"); close(); return false; } printMessage(1,"attaching sensors to fingers ...\n"); fingers[0].attachSensor(sensIF[0]); fingers[0].attachSensor(sensPort[0]); fingers[0].attachSensor(sensPort[1]); fingers[1].attachSensor(sensIF[1]); fingers[1].attachSensor(sensPort[2]); fingers[1].attachSensor(sensPort[3]); fingers[2].attachSensor(sensIF[2]); fingers[2].attachSensor(sensPort[4]); fingers[2].attachSensor(sensPort[5]); fingers[3].attachSensor(sensIF[3]); fingers[3].attachSensor(sensPort[6]); fingers[3].attachSensor(sensPort[7]); fingers[3].attachSensor(sensPort[8]); fingers[4].attachSensor(sensIF[4]); fingers[4].attachSensor(sensPort[9]); fingers[4].attachSensor(sensPort[10]); fingers[4].attachSensor(sensPort[11]); attachNode(fingers[0]); attachNode(fingers[1]); attachNode(fingers[2]); attachNode(fingers[3]); attachNode(fingers[4]); printMessage(1,"configuration complete\n"); return configured=true; }
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"); printf("type [get] and one of the following:\n"); printf("[%s] to read the number of controlled axes\n", Vocab::decode(VOCAB_AXES).c_str()); printf("[%s] to read the encoder value for all axes\n", Vocab::decode(VOCAB_ENCODERS).c_str()); printf("[%s] <int> to read the PID values for a single axis\n", Vocab::decode(VOCAB_PID).c_str()); printf("[%s] <int> to read the limit values for a single axis\n", Vocab::decode(VOCAB_LIMITS).c_str()); printf("[%s] to read the PID error for all axes\n", Vocab::decode(VOCAB_ERRS).c_str()); printf("[%s] to read the PID output for all axes\n", Vocab::decode(VOCAB_OUTPUTS).c_str()); printf("[%s] to read the reference position for all axes\n", Vocab::decode(VOCAB_REFERENCES).c_str()); printf("[%s] to read the reference speed for all axes\n", Vocab::decode(VOCAB_REF_SPEEDS).c_str()); printf("[%s] to read the reference acceleration for all axes\n", Vocab::decode(VOCAB_REF_ACCELERATIONS).c_str()); printf("[%s] to read the current consumption for all axes\n", Vocab::decode(VOCAB_AMP_CURRENTS).c_str()); printf("\n"); printf("type [set] and one of the following:\n"); printf("[%s] <int> <double> to move a single axis\n", Vocab::decode(VOCAB_POSITION_MOVE).c_str()); printf("[%s] <int> <double> to accelerate a single axis to a given speed\n", Vocab::decode(VOCAB_VELOCITY_MOVE).c_str()); printf("[%s] <int> <double> to set the reference speed for a single axis\n", Vocab::decode(VOCAB_REF_SPEED).c_str()); printf("[%s] <int> <double> to set the reference acceleration for a single axis\n", Vocab::decode(VOCAB_REF_ACCELERATION).c_str()); printf("[%s] <list> to move multiple axes\n", Vocab::decode(VOCAB_POSITION_MOVES).c_str()); printf("[%s] <list> to accelerate multiple axes to a given speed\n", Vocab::decode(VOCAB_VELOCITY_MOVES).c_str()); printf("[%s] <list> to set the reference speed for all axes\n", Vocab::decode(VOCAB_REF_SPEEDS).c_str()); printf("[%s] <list> to set the reference acceleration for all axes\n", Vocab::decode(VOCAB_REF_ACCELERATIONS).c_str()); printf("[%s] <int> to stop a single axis\n", Vocab::decode(VOCAB_STOP).c_str()); printf("[%s] <int> to stop all axes\n", Vocab::decode(VOCAB_STOPS).c_str()); printf("[%s] <int> <list> to set the PID values for a single axis\n", Vocab::decode(VOCAB_PID).c_str()); printf("[%s] <int> <list> to set the limits for a single axis\n", Vocab::decode(VOCAB_LIMITS).c_str()); printf("[%s] <int> to disable the PID control for a single axis\n", Vocab::decode(VOCAB_DISABLE).c_str()); printf("[%s] <int> to enable the PID control for a single axis\n", Vocab::decode(VOCAB_ENABLE).c_str()); printf("[%s] <int> <double> to set the encoder value for a single axis\n", Vocab::decode(VOCAB_ENCODER).c_str()); printf("[%s] <list> to set the encoder value for all axes\n", Vocab::decode(VOCAB_ENCODERS).c_str()); printf("\n"); break; case VOCAB_QUIT: goto ApplicationCleanQuit; break; case VOCAB_GET: switch(p.get(1).asVocab()) { case VOCAB_AXES: { int nj = 0; enc->getAxes(&nj); printf ("%s: %d\n", Vocab::decode(VOCAB_AXES).c_str(), nj); } break; case VOCAB_ENCODERS: { enc->getEncoders(tmp); printf ("%s: (", Vocab::decode(VOCAB_ENCODERS).c_str()); for(i = 0; i < jnts; i++) printf ("%.2f ", tmp[i]); printf (")\n"); } break; case VOCAB_PID: { Pid pd; int j = p.get(2).asInt(); pid->getPid(j, &pd); printf("%s: ", Vocab::decode(VOCAB_PID).c_str()); printf("kp %.2f ", pd.kp); printf("kd %.2f ", pd.kd); printf("ki %.2f ", pd.ki); printf("maxi %.2f ", pd.max_int); printf("maxo %.2f ", pd.max_output); printf("off %.2f ", pd.offset); printf("scale %.2f ", pd.scale); printf("\n"); } break; case VOCAB_LIMITS: { double min, max; int j = p.get(2).asInt(); lim->getLimits(j, &min, &max); printf("%s: ", Vocab::decode(VOCAB_LIMITS).c_str()); printf("limits: (%.2f %.2f)\n", min, max); } break; case VOCAB_ERRS: { pid->getErrorLimits(tmp); printf ("%s: (", Vocab::decode(VOCAB_ERRS).c_str()); for(i = 0; i < jnts; i++) printf ("%.2f ", tmp[i]); printf (")\n"); } break; case VOCAB_OUTPUTS: { pid->getErrors(tmp); printf ("%s: (", Vocab::decode(VOCAB_OUTPUTS).c_str()); for(i = 0; i < jnts; i++) printf ("%.2f ", tmp[i]); printf (")\n"); } break; case VOCAB_REFERENCES: { pid->getReferences(tmp); printf ("%s: (", Vocab::decode(VOCAB_REFERENCES).c_str()); for(i = 0; i < jnts; i++) printf ("%.2f ", tmp[i]); printf (")\n"); } break; case VOCAB_REF_SPEEDS: { pos->getRefSpeeds(tmp); printf ("%s: (", Vocab::decode(VOCAB_REF_SPEEDS).c_str()); for(i = 0; i < jnts; i++) printf ("%.2f ", tmp[i]); printf (")\n"); } break; case VOCAB_REF_ACCELERATIONS: { pos->getRefAccelerations(tmp); printf ("%s: (", Vocab::decode(VOCAB_REF_ACCELERATIONS).c_str()); for(i = 0; i < jnts; i++) printf ("%.2f ", tmp[i]); printf (")\n"); } break; case VOCAB_AMP_CURRENTS: { amp->getCurrents(tmp); printf ("%s: (", Vocab::decode(VOCAB_AMP_CURRENTS).c_str()); for(i = 0; i < jnts; i++) printf ("%.2f ", tmp[i]); printf (")\n"); } break; } break; case VOCAB_SET: switch(p.get(1).asVocab()) { case VOCAB_POSITION_MOVE: { int j = p.get(2).asInt(); double ref = p.get(3).asDouble(); printf("%s: moving %d to %.2f\n", Vocab::decode(VOCAB_POSITION_MOVE).c_str(), j, ref); pos->positionMove(j, ref); } break; case VOCAB_VELOCITY_MOVE: { int j = p.get(2).asInt(); double ref = p.get(3).asDouble(); printf("%s: accelerating %d to %.2f\n", Vocab::decode(VOCAB_VELOCITY_MOVE).c_str(), j, ref); vel->velocityMove(j, ref); } break; case VOCAB_REF_SPEED: { int j = p.get(2).asInt(); double ref = p.get(3).asDouble(); printf("%s: setting speed for %d to %.2f\n", Vocab::decode(VOCAB_REF_SPEED).c_str(), j, ref); pos->setRefSpeed(j, ref); } break; case VOCAB_REF_ACCELERATION: { int j = p.get(2).asInt(); double ref = p.get(3).asDouble(); printf("%s: setting acceleration for %d to %.2f\n", Vocab::decode(VOCAB_REF_ACCELERATION).c_str(), j, ref); pos->setRefAcceleration(j, ref); } break; case VOCAB_POSITION_MOVES: { Bottle *l = p.get(2).asList(); for (i = 0; i < jnts; i++) { tmp[i] = l->get(i).asDouble(); } printf("%s: moving all joints\n", Vocab::decode(VOCAB_POSITION_MOVES).c_str()); pos->positionMove(tmp); } break; case VOCAB_VELOCITY_MOVES: { Bottle *l = p.get(2).asList(); for (i = 0; i < jnts; i++) { tmp[i] = l->get(i).asDouble(); } printf("%s: moving all joints\n", Vocab::decode(VOCAB_VELOCITY_MOVES).c_str()); vel->velocityMove(tmp); } break; case VOCAB_REF_SPEEDS: { Bottle *l = p.get(2).asList(); for (i = 0; i < jnts; i++) { tmp[i] = l->get(i).asDouble(); } printf("%s: setting speed for all joints\n", Vocab::decode(VOCAB_REF_SPEEDS).c_str()); pos->setRefSpeeds(tmp); } break; case VOCAB_REF_ACCELERATIONS: { Bottle *l = p.get(2).asList(); for (i = 0; i < jnts; i++) { tmp[i] = l->get(i).asDouble(); } printf("%s: setting acceleration for all joints\n", Vocab::decode(VOCAB_REF_ACCELERATIONS).c_str()); pos->setRefAccelerations(tmp); } break; case VOCAB_STOP: { int j = p.get(2).asInt(); printf("%s: stopping axis %d\n", Vocab::decode(VOCAB_STOP).c_str()); pos->stop(j); } break; case VOCAB_STOPS: { printf("%s: stopping all axes %d\n", Vocab::decode(VOCAB_STOPS).c_str()); pos->stop(); } break; case VOCAB_ENCODER: { int j = p.get(2).asInt(); double ref = p.get(3).asDouble(); printf("%s: setting the encoder value for %d to %.2f\n", Vocab::decode(VOCAB_ENCODER).c_str(), j, ref); enc->setEncoder(j, ref); } break; case VOCAB_ENCODERS: { Bottle *l = p.get(2).asList(); for (i = 0; i < jnts; i++) { tmp[i] = l->get(i).asDouble(); } printf("%s: setting the encoder value for all joints\n", Vocab::decode(VOCAB_ENCODERS).c_str()); enc->setEncoders(tmp); } break; case VOCAB_PID: { Pid pd; int j = p.get(2).asInt(); Bottle *l = p.get(3).asList(); pd.kp = l->get(0).asDouble(); pd.kd = l->get(1).asDouble(); pd.ki = l->get(2).asDouble(); pd.max_int = l->get(3).asDouble(); pd.max_output = l->get(4).asDouble(); pd.offset = l->get(5).asDouble(); pd.scale = l->get(6).asDouble(); printf("%s: setting PID values for axis %d\n", Vocab::decode(VOCAB_PID).c_str(), j); pid->setPid(j, pd); } break; case VOCAB_DISABLE: { int j = p.get(2).asInt(); printf("%s: disabling control for axis %d\n", Vocab::decode(VOCAB_DISABLE).c_str(), j); pid->disablePid(j); amp->disableAmp(j); } break; case VOCAB_ENABLE: { int j = p.get(2).asInt(); printf("%s: enabling control for axis %d\n", Vocab::decode(VOCAB_ENABLE).c_str(), j); amp->enableAmp(j); pid->enablePid(j); } break; case VOCAB_LIMITS: { int j = p.get(2).asInt(); printf("%s: setting limits for axis %d\n", Vocab::decode(VOCAB_LIMITS).c_str(), j); Bottle *l = p.get(3).asList(); lim->setLimits(j, l->get(0).asDouble(), l->get(1).asDouble()); } break; } break; } /* switch get(0) */ } /* while () */ ApplicationCleanQuit: dd.close(); delete[] tmp; return 0; }
virtual bool configure(ResourceFinder &rf) { // get command line options if (!rf.check("robot") || !rf.check("part")) { printf("Missing either --robot or --part options. Quitting!\n"); return false; } std::string dumpername; // get dumepr name if (rf.check("name")) { dumpername = rf.find("name").asString(); dumpername += "/"; } else { dumpername = "/controlBoardDumper/"; } Value& robot = rf.find("robot"); Value& part = rf.find("part"); configFileRobotPart = "config_"; configFileRobotPart = configFileRobotPart + robot.asString(); configFileRobotPart = configFileRobotPart + "_"; configFileRobotPart = configFileRobotPart + part.asString(); configFileRobotPart = configFileRobotPart + ".ini"; //get the joints to be dumped getRate(rf, rate); if (getNumberUsedJoints(rf, nJoints) == 0) return false; thetaMap = new int[nJoints]; if (getUsedJointsMap(rf, nJoints, thetaMap) == 0) return false; //get the type of data to be dumped if (getNumberDataToDump(rf, nData) == 0) return false; dataToDump = new std::string[nData]; if (getDataToDump(rf, dataToDump, nData, &useDebugClient) == 0) return false; // Printing configuration to the user yInfo("Running with the following configuration:\n"); yInfo("Selected rate is: %d\n", rate); yInfo("Data selected to be dumped are:\n"); for (int i = 0; i < nData; i++) yInfo("\t%s \n", dataToDump[i].c_str()); //open remote control board ddBoardOptions.put("device", "remote_controlboard"); ddDebugOptions.put("device", "debugInterfaceClient"); std::string localPortName = name; std::string localDebugPortName = name; localPortName = localPortName + dumpername; localDebugPortName = localPortName + "debug/"; //localPortName = localPortName + robot.asString(); localPortName = localPortName + part.asString(); localDebugPortName = localDebugPortName + part.asString(); ddBoardOptions.put("local", localPortName); ddDebugOptions.put("local", localDebugPortName); std::string remotePortName = "/"; std::string remoteDebugPortName; remotePortName = remotePortName + robot.asString(); remotePortName = remotePortName + "/"; remotePortName = remotePortName + part.asString(); ddBoardOptions.put("remote", remotePortName); remoteDebugPortName = remotePortName + "/debug"; ddDebugOptions.put("remote", remoteDebugPortName); // create a device ddBoard.open(ddBoardOptions); if (!ddBoard.isValid()) { printf("Device not available.\n"); return false; } if (useDebugClient ) { ddDebug.open(ddDebugOptions); if (!ddDebug.isValid()) { yError("\n-----------------------------------------\n"); yError("Debug Interface is mandatory to run this module with the '--dataToDumpAll' option or to dump any of the getRotorxxx data.\n"); yError("Please Verify the following 2 conditions are satisfied:\n\n"); yError("1) Check 'debugInterfaceClient' is available using 'yarpdev --list' command\n"); // yError("%s", Drivers::factory().toString().c_str()); std::string deviceList, myDev; deviceList.clear(); deviceList.append(Drivers::factory().toString()); myDev = "debugInterfaceClient"; if(deviceList.find(myDev) != std::string::npos) yError("\t--> Seems OK\n"); else yError("\t--> Seems NOT OK. The device was not found, please activate the compilation using the corrisponding CMake flag.\n"); yError("\n2) Check if the robot has the 'debugInterfaceWrapper' device up and running. \n You should see from 'yarp name list' output, a port called\n"); yError("\t/robotName/part_name/debug/rpc:i\n If not, fix the robot configuration files to instantiate the 'debugInterfaceWrapper' device.\n"); yError("\nQuickFix: If you set the --dataToDumpAll and do not need the advanced debug feature (getRotorxxx) just remove this option. See help for more information.\n"); yError("------------- END ERROR MESSAGE ---------------\n\n"); return false; } } bool logToFile = false; if (rf.check("logToFile")) logToFile = true; portPrefix= dumpername + part.asString() + "/"; //boardDumperThread *myDumper = new boardDumperThread(&dd, rate, portPrefix, dataToDump[0]); //myDumper->setThetaMap(thetaMap, nJoints); myDumper = new boardDumperThread[nData]; for (int i = 0; i < nData; i++) { if (dataToDump[i] == "getEncoders" ) { if (ddBoard.view(ienc)) { yInfo("Initializing a getEncs thread\n"); myDumper[i].setDevice(&ddBoard, &ddDebug, rate, portPrefix, dataToDump[i], logToFile); myDumper[i].setThetaMap(thetaMap, nJoints); myGetEncs.setInterface(ienc); if (ddBoard.view(istmp)) { yInfo("getEncoders::The time stamp initalization interfaces was successfull! \n"); myGetEncs.setStamp(istmp); } else yError("Problems getting the time stamp interfaces \n"); myDumper[i].setGetter(&myGetEncs); } } else if (dataToDump[i] == "getEncoderSpeeds") { if (ddBoard.view(ienc)) { yInfo("Initializing a getSpeeds thread\n"); myDumper[i].setDevice(&ddBoard, &ddDebug, rate, portPrefix, dataToDump[i], logToFile); myDumper[i].setThetaMap(thetaMap, nJoints); myGetSpeeds.setInterface(ienc); if (ddBoard.view(istmp)) { yInfo("getEncodersSpeed::The time stamp initalization interfaces was successfull! \n"); myGetSpeeds.setStamp(istmp); } else yError("Problems getting the time stamp interfaces \n"); myDumper[i].setGetter(&myGetSpeeds); } } else if (dataToDump[i] == "getEncoderAccelerations") { if (ddBoard.view(ienc)) { yInfo("Initializing a getAccs thread\n"); myDumper[i].setDevice(&ddBoard, &ddDebug, rate, portPrefix, dataToDump[i], logToFile); myDumper[i].setThetaMap(thetaMap, nJoints); myGetAccs.setInterface(ienc); if (ddBoard.view(istmp)) { yInfo("getEncoderAccelerations::The time stamp initalization interfaces was successfull! \n"); myGetAccs.setStamp(istmp); } else yError("Problems getting the time stamp interfaces \n"); myDumper[i].setGetter(&myGetAccs); } } else if (dataToDump[i] == "getPosPidReferences") { if (ddBoard.view(ipid)) { yInfo("Initializing a getErrs thread\n"); myDumper[i].setDevice(&ddBoard, &ddDebug, rate, portPrefix, dataToDump[i], logToFile); myDumper[i].setThetaMap(thetaMap, nJoints); myGetPidRefs.setInterface(ipid); if (ddBoard.view(istmp)) { yInfo("getPosPidReferences::The time stamp initalization interfaces was successfull! \n"); myGetPidRefs.setStamp(istmp); } else yError("Problems getting the time stamp interfaces \n"); myDumper[i].setGetter(&myGetPidRefs); } } else if (dataToDump[i] == "getTrqPidReferences") { if (ddBoard.view(itrq)) { yInfo("Initializing a getErrs thread\n"); myDumper[i].setDevice(&ddBoard, &ddDebug, rate, portPrefix, dataToDump[i], logToFile); myDumper[i].setThetaMap(thetaMap, nJoints); myGetTrqRefs.setInterface(itrq); if (ddBoard.view(istmp)) { yInfo("getTrqPidReferences::The time stamp initalization interfaces was successfull! \n"); myGetTrqRefs.setStamp(istmp); } else yError("Problems getting the time stamp interfaces \n"); myDumper[i].setGetter(&myGetTrqRefs); } } else if (dataToDump[i] == "getControlModes") { if (ddBoard.view(icmod)) { yInfo("Initializing a getErrs thread\n"); myDumper[i].setDevice(&ddBoard, &ddDebug, rate, portPrefix, dataToDump[i], logToFile); myDumper[i].setThetaMap(thetaMap, nJoints); myGetControlModes.setInterface(icmod, nJoints); if (ddBoard.view(istmp)) { yInfo("getControlModes::The time stamp initalization interfaces was successfull! \n"); myGetControlModes.setStamp(istmp); } else yError("Problems getting the time stamp interfaces \n"); myDumper[i].setGetter(&myGetControlModes); } } else if (dataToDump[i] == "getInteractionModes") { if (ddBoard.view(iimod)) { yInfo("Initializing a getErrs thread\n"); myDumper[i].setDevice(&ddBoard, &ddDebug, rate, portPrefix, dataToDump[i], logToFile); myDumper[i].setThetaMap(thetaMap, nJoints); myGetInteractionModes.setInterface(iimod, nJoints); if (ddBoard.view(istmp)) { yInfo("getInteractionModes::The time stamp initalization interfaces was successfull! \n"); myGetInteractionModes.setStamp(istmp); } else yError("Problems getting the time stamp interfaces \n"); myDumper[i].setGetter(&myGetInteractionModes); } } else if (dataToDump[i] == "getPositionErrors") { if (ddBoard.view(ipid)) { yInfo("Initializing a getErrs thread\n"); myDumper[i].setDevice(&ddBoard, &ddDebug, rate, portPrefix, dataToDump[i], logToFile); myDumper[i].setThetaMap(thetaMap, nJoints); myGetPosErrs.setInterface(ipid); if (ddBoard.view(istmp)) { yInfo("getPositionErrors::The time stamp initalization interfaces was successfull! \n"); myGetPosErrs.setStamp(istmp); } else yError("Problems getting the time stamp interfaces \n"); myDumper[i].setGetter(&myGetPosErrs); } } else if (dataToDump[i] == "getOutputs") { if (ddBoard.view(ipid)) { yInfo("Initializing a getOuts thread\n"); myDumper[i].setDevice(&ddBoard, &ddDebug, rate, portPrefix, dataToDump[i], logToFile); myDumper[i].setThetaMap(thetaMap, nJoints); myGetOuts.setInterface(ipid); if (ddBoard.view(istmp)) { yInfo("getOutputs::The time stamp initalization interfaces was successfull! \n"); myGetOuts.setStamp(istmp); } else yError("Problems getting the time stamp interfaces \n"); myDumper[i].setGetter(&myGetOuts); } } else if (dataToDump[i] == "getCurrents") { if (ddBoard.view(iamp)) { yInfo("Initializing a getCurrs thread\n"); myDumper[i].setDevice(&ddBoard, &ddDebug, rate, portPrefix, dataToDump[i], logToFile); myDumper[i].setThetaMap(thetaMap, nJoints); myGetCurrs.setInterface(iamp); if (ddBoard.view(istmp)) { yInfo("getCurrents::The time stamp initalization interfaces was successfull! \n"); myGetCurrs.setStamp(istmp); } else yError("Problems getting the time stamp interfaces \n"); myDumper[i].setGetter(&myGetCurrs); } } else if (dataToDump[i] == "getTorques") { if (ddBoard.view(itrq)) { yInfo("Initializing a getTorques thread\n"); myDumper[i].setDevice(&ddBoard, &ddDebug, rate, portPrefix, dataToDump[i], logToFile); myDumper[i].setThetaMap(thetaMap, nJoints); myGetTrqs.setInterface(itrq); if (ddBoard.view(istmp)) { yInfo("getTorques::The time stamp initalization interfaces was successfull! \n"); myGetTrqs.setStamp(istmp); } else yError("Problems getting the time stamp interfaces \n"); myDumper[i].setGetter(&myGetTrqs); } } else if (dataToDump[i] == "getTorqueErrors") { if (ddBoard.view(ipid)) { yInfo("Initializing a getTorqueErrors thread\n"); myDumper[i].setDevice(&ddBoard, &ddDebug, rate, portPrefix, dataToDump[i], logToFile); myDumper[i].setThetaMap(thetaMap, nJoints); myGetTrqErrs.setInterface(ipid); if (ddBoard.view(istmp)) { yInfo("getTorqueErrors::The time stamp initalization interfaces was successfull! \n"); myGetTrqErrs.setStamp(istmp); } else yError("Problems getting the time stamp interfaces \n"); myDumper[i].setGetter(&myGetTrqErrs); } } else if (dataToDump[i] == "getTemperatures") { if (ddBoard.view(imotenc)) { 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; }