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() { ImageOf<PixelRgb> *imgIn=imgInPort.read(true); if (imgIn==NULL) return false; ImageOf<PixelRgb> &imgOut=imgOutPort.prepare(); imgOut=*imgIn; cv::Mat img=cv::cvarrToMat(imgOut.getIplImage()); Vector xa,oa; iarm->getPose(xa,oa); Matrix Ha=axis2dcm(oa); xa.push_back(1.0); Ha.setCol(3,xa); Vector pc; igaze->get2DPixel(camSel,xa,pc); cv::Point point_c((int)pc[0],(int)pc[1]); cv::circle(img,point_c,4,cv::Scalar(0,255,0),4); Vector analogs,encs(nEncs),joints; if (ianalog!=NULL) ianalog->read(analogs); iencs->getEncoders(encs.data()); for (int i=0; i<3; i++) { if (ianalog!=NULL) finger[i].getChainJoints(encs,analogs,joints); else finger[i].getChainJoints(encs,joints); finger[i].setAng(CTRL_DEG2RAD*joints); } for (int fng=0; fng<3; fng++) { deque<cv::Point> point_f; for (int i=-1; i<(int)finger[fng].getN(); i++) { Vector fc; igaze->get2DPixel(camSel,Ha*(i<0?finger[fng].getH0().getCol(3): finger[fng].getH(i,true).getCol(3)),fc); point_f.push_front(cv::Point((int)fc[0],(int)fc[1])); cv::circle(img,point_f.front(),3,cv::Scalar(0,0,255),4); if (i>=0) { cv::line(img,point_f.front(),point_f.back(),cv::Scalar(255,255,255),2); point_f.pop_back(); } else cv::line(img,point_c,point_f.front(),cv::Scalar(255,0,0),2); } } imgOutPort.writeStrict(); return true; }
virtual void run() { while (isStopping() != true) { /* poll the click ports containers to see if we have left/right ready to go */ bfL.lock(); bfR.lock(); if (bfL.size() == 2 && bfR.size() == 2) { printf("got a hit!\n"); /* if they are, raise the flag that action is beginning, save current joint configuration */ Bottle susmsg; susmsg.addInt(1); susPort->write(susmsg); //get the current joint configuration for the torso, head, and arm tang.clear(); tang.resize(3); tEnc->getEncoders(tang.data()); hang.clear(); hang.resize(6); hEnc->getEncoders(hang.data()); aang.clear(); aang.resize(16); aEnc->getEncoders(aang.data()); /* get the xyz location of the gaze point */ Vector bvL(2); Vector bvR(2); bvL[0] = bfL.get(0).asDouble(); bvL[1] = bfL.get(1).asDouble(); bvR[0] = bfR.get(0).asDouble(); bvR[1] = bfR.get(1).asDouble(); objPos.clear(); objPos.resize(3); gaze->triangulate3DPoint(bvL,bvR,objPos); /* servo the head to gaze at that point */ //gaze->lookAtStereoPixels(bvL,bvR); gaze->lookAtFixationPoint(objPos); gaze->waitMotionDone(1.0,10.0); gaze->stopControl(); printf("object position estimated as: %f, %f, %f\n", objPos[0], objPos[1], objPos[2]); printf("is this ok?\n"); string posResp = Network::readString().c_str(); if (posResp == "yes" || posResp == "y") { /* move to hover the hand over the XY position of the target: [X, Y, Z=0.2], with palm upright */ objPos[2] = 0.1; Vector axa(4); axa.zero(); if (armInUse) { axa[2] = 1.0; axa[3] = M_PI; } else { axa[1] = 1.0; axa[3] = M_PI; } carm->goToPoseSync(objPos,axa); carm->waitMotionDone(1.0,10.0); Time::delay(2.0); //curl fingers and thumb slightly to hold object Vector armCur(16); aEnc->getEncoders(armCur.data()); armCur[8] = 3; armCur[10] = 25; armCur[11] = 25; armCur[12] = 25; armCur[13] = 25; armCur[14] = 25; armCur[15] = 55; aPos->positionMove(armCur.data()); Time::delay(2.0); /* wait for terminal signal from user that object has been moved to the hand */ bool validTarg = false; printf("object position reached, place in hand and enter target xy position\n"); while (!validTarg) { string objResp = Network::readString().c_str(); /* ask the user to enter in an XY target location, or confirm use of previous one */ Bottle btarPos(objResp.c_str()); if (btarPos.size() < 2) { //if user enters no target position, try to use last entered position if (targetPos.length() != 3) { printf("no previous target position available, please re-enter:\n"); } else { validTarg = true; } } else { targetPos.clear(); targetPos.resize(3); targetPos[0] = btarPos.get(0).asDouble(); targetPos[1] = btarPos.get(1).asDouble(); targetPos[2] = 0.1; validTarg = true; } } /* move the arm to above the target location */ axa.zero(); if (armInUse) { axa[2] = 1.0; axa[3] = M_PI; } else { axa[1] = 1.0; axa[3] = M_PI; } carm->goToPoseSync(targetPos,axa); //carm->goToPosition(targetPos); carm->waitMotionDone(1.0,10.0); Time::delay(2.0); /* wait for user signal that the object has been removed */ printf("object has been moved to target location. please remove object and hit enter\n"); string tarResp = Network::readString().c_str(); } /* return to saved motor configuration, clear click buffers, lower flag signaling action done */ printf("gaze done, attempting reset\n"); tPos->positionMove(tang.data()); hPos->positionMove(hang.data()); aPos->positionMove(aang.data()); bfL.clear(); bfR.clear(); bfL.unlock(); bfR.unlock(); susmsg.clear(); susmsg.addInt(0); susPort->write(susmsg); } else { bfL.unlock(); bfR.unlock(); } } }
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; bool done = false; bool ret = false; ipos->getAxes(&NUMBER_OF_JOINTS); if (NUMBER_OF_JOINTS == 0) { fprintf(stderr,"Lost connection with iCubInterface. You should save and restart.\n" ); Time::delay(0.1); pColor=&color_grey; strcpy(frame_title,"DISCONNECTED"); for (k = 0; k < MAX_NUMBER_OF_JOINTS; k++) { if (currentPart->framesArray[k]!=0) { gtk_frame_set_label (GTK_FRAME(currentPart->framesArray[k]),frame_title); gtk_widget_modify_bg (colorback[k], GTK_STATE_NORMAL, pColor); } } return true; } for (k = 0; k < NUMBER_OF_JOINTS; k++) { max_torques[k]=0; min_torques[k]=0; torques[k]=0; } if (!iiencs->getEncoders(positions)) return true; itrq->getTorques(torques); iiencs->getEncoderSpeeds(speeds); //update all joints positions for (k = 0; k < NUMBER_OF_JOINTS; k++) { sprintf(buffer, "%.1f", positions[k]); gtk_entry_set_text((GtkEntry*) pos_entry[k], buffer); sprintf(buffer, "%.3f", torques[k]); gtk_entry_set_text((GtkEntry*) trq_entry[k], buffer); sprintf(buffer, "%.1f", speeds[k]); gtk_entry_set_text((GtkEntry*) speed_entry[k], buffer); } //update all joint sliders for (k = 0; k < NUMBER_OF_JOINTS; k++) if(POS_UPDATE[k]) gtk_range_set_value((GtkRange*)sliderAry[k], positions[k]); // *** update the checkMotionDone box section *** // (only one at a time in order to save badwidth) k = slowSwitcher%NUMBER_OF_JOINTS; slowSwitcher++; #if DEBUG_GUI gtk_entry_set_text((GtkEntry*) inEntry[k], "off"); #else ipos->checkMotionDone(k, &done); if (!done) gtk_entry_set_text((GtkEntry*) inEntry[k], " "); else gtk_entry_set_text((GtkEntry*) inEntry[k], "@"); #endif // *** update the controlMode section *** // the new icubinterface does not increase the bandwidth consumption // ret = true; useless guys! ret=ictrl->getControlModes(controlModes); if (ret==false) fprintf(stderr,"ictrl->getControlMode failed\n" ); ret=iint->getInteractionModes(interactionModes); if (ret==false) fprintf(stderr,"iint->getInteractionlMode failed\n" ); for (k = 0; k < NUMBER_OF_JOINTS; k++) { if (currentPart->first_time==false && controlModes[k] == controlModesOld[k]) continue; controlModesOld[k]=controlModes[k]; sprintf(frame_title,"Joint %d ",k ); switch (controlModes[k]) { case VOCAB_CM_IDLE: pColor=&color_yellow; strcat(frame_title," (IDLE)"); gtk_frame_set_label (GTK_FRAME(currentPart->framesArray[k]),frame_title); gtk_widget_modify_bg (colorback[k], GTK_STATE_NORMAL, pColor); break; case VOCAB_CM_POSITION: pColor=&color_green; strcat(frame_title," (POSITION)"); gtk_frame_set_label (GTK_FRAME(currentPart->framesArray[k]),frame_title); gtk_frame_set_label (GTK_FRAME(currentPart->frame_slider1[k]),"Position:"); gtk_frame_set_label (GTK_FRAME(currentPart->frame_slider2[k]),"Velocity:"); gtk_widget_modify_bg (colorback[k], GTK_STATE_NORMAL, pColor); break; case VOCAB_CM_POSITION_DIRECT: pColor=&color_dark_green; strcat(frame_title," (POSITION_DIRECT)"); gtk_frame_set_label (GTK_FRAME(currentPart->framesArray[k]),frame_title); gtk_frame_set_label (GTK_FRAME(currentPart->frame_slider1[k]),"Position:"); gtk_frame_set_label (GTK_FRAME(currentPart->frame_slider2[k]),"---"); gtk_widget_modify_bg (colorback[k], GTK_STATE_NORMAL, pColor); break; case VOCAB_CM_MIXED: pColor=&color_green_blue; strcat(frame_title," (MIXED_MODE)"); gtk_frame_set_label (GTK_FRAME(currentPart->framesArray[k]),frame_title); gtk_frame_set_label (GTK_FRAME(currentPart->frame_slider1[k]),"Position:"); gtk_frame_set_label (GTK_FRAME(currentPart->frame_slider2[k]),"Velocity"); gtk_widget_modify_bg (colorback[k], GTK_STATE_NORMAL, pColor); break; case VOCAB_CM_VELOCITY: pColor=&color_blue; strcat(frame_title," (VELOCITY)"); gtk_frame_set_label (GTK_FRAME(currentPart->framesArray[k]),frame_title); gtk_widget_modify_bg (colorback[k], GTK_STATE_NORMAL, pColor); break; case VOCAB_CM_TORQUE: pColor=&color_pink; strcat(frame_title," (TORQUE)"); gtk_frame_set_label (GTK_FRAME(currentPart->framesArray[k]),frame_title); gtk_frame_set_label (GTK_FRAME(currentPart->frame_slider1[k]),"Torque:"); gtk_frame_set_label (GTK_FRAME(currentPart->frame_slider2[k]),"Torque2:"); gtk_widget_modify_bg (colorback[k], GTK_STATE_NORMAL, pColor); break; case VOCAB_CM_IMPEDANCE_POS: pColor=&color_indaco; strcat(frame_title," (IMPEDANCE POS)"); gtk_frame_set_label (GTK_FRAME(currentPart->framesArray[k]),frame_title); gtk_widget_modify_bg (colorback[k], GTK_STATE_NORMAL, pColor); break; case VOCAB_CM_IMPEDANCE_VEL: pColor=&color_indaco; strcat(frame_title," (IMPEDANCE VEL)"); gtk_frame_set_label (GTK_FRAME(currentPart->framesArray[k]),frame_title); gtk_widget_modify_bg (colorback[k], GTK_STATE_NORMAL, pColor); break; case VOCAB_CM_OPENLOOP: pColor=&color_white; strcat(frame_title," (OPENLOOP)"); gtk_frame_set_label (GTK_FRAME(currentPart->framesArray[k]),frame_title); gtk_widget_modify_bg (colorback[k], GTK_STATE_NORMAL, pColor); break; case VOCAB_CM_HW_FAULT: pColor=&color_fault_red; strcat(frame_title," (HARDWARE_FAULT)"); gtk_frame_set_label (GTK_FRAME(currentPart->framesArray[k]),frame_title); gtk_frame_set_label (GTK_FRAME(currentPart->frame_slider1[k]),"---"); gtk_frame_set_label (GTK_FRAME(currentPart->frame_slider2[k]),"---"); gtk_widget_modify_bg (colorback[k], GTK_STATE_NORMAL, pColor); break; case VOCAB_CM_CALIBRATING: pColor=&color_grey; strcat(frame_title," (CALIBRATING)"); gtk_frame_set_label (GTK_FRAME(currentPart->framesArray[k]),frame_title); gtk_widget_modify_bg (colorback[k], GTK_STATE_NORMAL, pColor); break; case VOCAB_CM_CALIB_DONE: pColor=&color_grey; strcat(frame_title," (CALIB DONE)"); gtk_frame_set_label (GTK_FRAME(currentPart->framesArray[k]),frame_title); gtk_widget_modify_bg (colorback[k], GTK_STATE_NORMAL, pColor); break; case VOCAB_CM_NOT_CONFIGURED: pColor=&color_grey; strcat(frame_title," (NOT CONFIGURED)"); gtk_frame_set_label (GTK_FRAME(currentPart->framesArray[k]),frame_title); gtk_widget_modify_bg (colorback[k], GTK_STATE_NORMAL, pColor); break; case VOCAB_CM_CONFIGURED: pColor=&color_grey; strcat(frame_title," (CONFIGURED)"); gtk_frame_set_label (GTK_FRAME(currentPart->framesArray[k]),frame_title); gtk_widget_modify_bg (colorback[k], GTK_STATE_NORMAL, pColor); break; default: case VOCAB_CM_UNKNOWN: pColor=&color_grey; strcat(frame_title," (UNKNOWN)"); gtk_frame_set_label (GTK_FRAME(currentPart->framesArray[k]),frame_title); gtk_widget_modify_bg (colorback[k], GTK_STATE_NORMAL, pColor); break; } } for (k = 0; k < NUMBER_OF_JOINTS; k++) { if (currentPart->first_time==false && interactionModes[k] == interactionModesOld[k]) continue; interactionModesOld[k]=interactionModes[k]; switch (interactionModes[k]) { case VOCAB_IM_STIFF: gtk_widget_modify_base ((GtkWidget*)inEntry[k], GTK_STATE_NORMAL, &color_green); break; case VOCAB_IM_COMPLIANT: gtk_widget_modify_base ((GtkWidget*)inEntry[k], GTK_STATE_NORMAL, &color_fault_red); break; default: case VOCAB_CM_UNKNOWN: gtk_widget_modify_base ((GtkWidget*)inEntry[k], GTK_STATE_NORMAL, &color_white); break; } } currentPart->first_time =false; return true; }
virtual void run(){ tmp = command; (*command)[0] = -60*(gsl_rng_uniform(r)); (*command)[1] = 100*(gsl_rng_uniform(r)); (*command)[2] = -35 + 95*(gsl_rng_uniform(r)); (*command)[3] = 10 + 90*(gsl_rng_uniform(r)); printf("%.1lf %.1lf %.1lf %.1lf\n", (*command)[0], (*command)[1], (*command)[2], (*command)[3]); //above 0 doesn't seem to be safe for joint 0 if ((*command)[0] > 0 || (*command)[0] < -60){ (*command)[0] = (*tmp)[0]; } if ((*command)[1] > 100 || (*command)[1] < -0){ (*command)[1] = (*tmp)[1]; } if ((*command)[2] > 60 || (*command)[2] < -35){ (*command)[2] = (*tmp)[2]; } if ((*command)[3] > 100 || (*command)[3] < 10){ (*command)[3] = (*tmp)[3]; } //use fwd kin to find end effector position Bottle plan, pred; for (int i = 0; i < nj; i++){ plan.add((*command)[i]); } armPlan->write(plan); armPred->read(pred); Vector commandCart(3); for (int i = 0; i < 3; i++){ commandCart[i] = pred.get(i).asDouble(); } printf("Cartesian safety check\n"); double rad = sqrt(commandCart[0]*commandCart[0]+commandCart[1]*commandCart[1]); // safety radius back to 30 cm if (rad > 0.3){ pos->positionMove(command->data()); bool done = false; while(!done){ pos->checkMotionDone(&done); Time::delay(0.1); } printf("Moved arm to new location\n"); Vector &armJ = armLocJ->prepare(); Vector encoders(nj); enc->getEncoders(encoders.data()); armJ = encoders; Vector noisyArm(3); for(int i = 0; i < 3; i++){ //noisyArm[i] = commandCart[i] + 0.01*(2*gsl_rng_uniform(r)-1); //sanity check noisyArm[i] = commandCart[i] + 0.005*(2*gsl_rng_uniform(r)-1); } //insert here: //read off peak saliences //fixate there //calculate cartesian value, compare to real cart. value of arm printf("Looking at arm\n"); igaze->lookAtFixationPoint(noisyArm); done = false; while(!done){ igaze->checkMotionDone(&done); Time::delay(0.5); } //igaze->waitMotionDone(0.1,30); printf("Saw arm\n"); Vector &headAng = headLoc->prepare(); igaze->getAngles(headAng); Bottle tStamp; tStamp.clear(); tStamp.add(Time::now()); headLoc->setEnvelope(tStamp); headLoc->write(); armLocJ->write(); headLoc->unprepare(); armLocJ->unprepare(); } else{ printf("Self collision detected!\n"); } }
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 --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[]) { // 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; }
int main(int argc, char const *argv[]) { Network yarp; Node node("/icub_sim/state_publisher"); Property params; params.fromCommand(argc, argv); if (!params.check("robot")) { fprintf(stderr, "Please specify the name of the robot\n"); fprintf(stderr, "--robot name (e.g. icub)\n"); return 1; } std::string robotName=params.find("robot").asString().c_str(); std::string remotePorts="/"; remotePorts+=robotName; std::string remoteRArmPorts = remotePorts + "/right_arm"; std::string remoteHeadPorts = remotePorts + "/head"; std::string remoteTorsoPorts = remotePorts + "/torso"; std::string remoteLArmPorts = remotePorts + "/left_arm"; Property options_head; options_head.put("device", "remote_controlboard"); options_head.put("local", "/head/client"); //local port names options_head.put("remote", remoteHeadPorts.c_str()); // create a device PolyDriver headDevice(options_head); if (!headDevice.isValid()) { printf("Device not available. Here are the known devices:\n"); printf("%s", Drivers::factory().toString().c_str()); return 0; } Property options_torso; options_torso.put("device", "remote_controlboard"); options_torso.put("local", "/torso/client"); //local port names options_torso.put("remote", remoteTorsoPorts.c_str()); // create a device PolyDriver torsoDevice(options_torso); if (!torsoDevice.isValid()) { printf("Device not available. Here are the known devices:\n"); printf("%s", Drivers::factory().toString().c_str()); return 0; } Property options_right_arm; options_right_arm.put("device", "remote_controlboard"); options_right_arm.put("local", "/right_arm/client"); //local port names options_right_arm.put("remote", remoteRArmPorts.c_str()); // create a device PolyDriver rArmDevice(options_right_arm); if (!rArmDevice.isValid()) { printf("Device not available. Here are the known devices:\n"); printf("%s", Drivers::factory().toString().c_str()); return 0; } Property options_left_arm; options_left_arm.put("device", "remote_controlboard"); options_left_arm.put("local", "/left_arm/client"); //local port names options_left_arm.put("remote", remoteLArmPorts.c_str()); // create a device PolyDriver lArmDevice(options_left_arm); if (!lArmDevice.isValid()) { printf("Device not available. Here are the known devices:\n"); printf("%s", Drivers::factory().toString().c_str()); return 0; } IPositionControl *posHead; IPositionControl *posTorso; IPositionControl *posRArm; IPositionControl *posLArm; IEncoders *encsHead; IEncoders *encsTorso; IEncoders *encsRArm; IEncoders *encsLArm; torsoDevice.view(posTorso); torsoDevice.view(encsTorso); rArmDevice.view(posRArm); rArmDevice.view(encsRArm); lArmDevice.view(posLArm); lArmDevice.view(encsLArm); headDevice.view(posHead); headDevice.view(encsHead); int nj = 0; posHead->getAxes(&nj); Vector encodersHead; encodersHead.resize(nj); nj = 0; posTorso->getAxes(&nj); Vector encodersTorso; encodersTorso.resize(nj); nj = 0; posRArm->getAxes(&nj); Vector encodersRArm; encodersRArm.resize(nj); nj = 0; posLArm->getAxes(&nj); Vector encodersLArm; encodersLArm.resize(nj); yarp::os::Publisher<sensor_msgs_JointState> joint_pub; if (!joint_pub.topic("/joint_states")) { std::cerr<< "Failed to create publisher to /chatter\n"; return -1; } float degtorad = 0.0174532925; sensor_msgs_JointState joint_states; struct timespec currentTime; joint_states.name.resize(70); joint_states.position.resize(70); joint_states.velocity.resize(70); joint_states.name[0] ="j1"; joint_states.position[0] = 0.0; joint_states.name[1] ="j2"; joint_states.position[1] = 0.0; joint_states.name[2] ="j3"; joint_states.position[2] = 0.0; joint_states.name[3] ="j4"; joint_states.position[3] = 0.0; joint_states.name[4] ="j5"; joint_states.position[4] = 0.0; joint_states.name[5] ="j6"; joint_states.position[5] = 0.0; joint_states.name[6] ="j7"; joint_states.position[6] = 0.0; joint_states.name[7] ="j8"; joint_states.position[7] = 0.0; joint_states.name[8] ="j7s"; joint_states.position[8] = 0.0; joint_states.name[9] ="j8s"; joint_states.position[9] = 0.0; joint_states.name[10] ="raj1"; joint_states.position[10] = 0.0; joint_states.name[11] ="raj2"; joint_states.position[11] = 0.0; joint_states.name[12] ="raj3"; joint_states.position[12] = 0.0; joint_states.name[13] ="raj4"; joint_states.position[13] = 0.0; joint_states.name[14] ="raj5"; joint_states.position[14] = 0.0; joint_states.name[15] ="raj6"; joint_states.position[15] = 0.0; joint_states.name[16] ="laj1"; joint_states.position[16] = 0.0; joint_states.name[17] ="laj2"; joint_states.position[17] = 0.0; joint_states.name[18] ="laj3"; joint_states.position[18] = 0.0; joint_states.name[19] ="laj4"; joint_states.position[19] = 0.0; joint_states.name[20] ="laj5"; joint_states.position[20] = 0.0; joint_states.name[21] ="laj6"; joint_states.position[21] = 0.0; joint_states.name[22] ="rlaj1"; joint_states.position[22] = 0.0; joint_states.name[23] ="rlaj2"; joint_states.position[23] = 0.0; joint_states.name[24] ="rlaj3"; joint_states.position[24] = 0.0; joint_states.name[25] ="rlaj4"; joint_states.position[25] = 0.0; joint_states.name[26] ="rlaj5"; joint_states.position[26] = 0.0; joint_states.name[27] ="rlaj6"; joint_states.position[27] = 0.0; joint_states.name[28] ="llaj1"; joint_states.position[28] = 0.0; joint_states.name[29] ="llaj2"; joint_states.position[29] = 0.0; joint_states.name[30] ="llaj3"; joint_states.position[30] = 0.0; joint_states.name[31] ="llaj4"; joint_states.position[31] = 0.0; joint_states.name[32] ="llaj5"; joint_states.position[32] = 0.0; joint_states.name[33] ="llaj6"; joint_states.position[33] = 0.0; joint_states.name[34] ="right_wrist_yaw"; joint_states.position[34] = 0.0; joint_states.name[35] ="tj2"; joint_states.position[35] = 0.0; joint_states.name[36] ="tj4"; joint_states.position[36] = 0.0; joint_states.name[37] ="tj5"; joint_states.position[37] = 0.0; joint_states.name[38] ="tj6"; joint_states.position[38] = 0.0; joint_states.name[39] ="ij3"; joint_states.position[39] = 0.0; joint_states.name[40] ="ij4"; joint_states.position[40] = 0.0; joint_states.name[41] ="ij5"; joint_states.position[41] = 0.0; joint_states.name[42] ="mj3"; joint_states.position[42] = 0.0; joint_states.name[43] ="mj4"; joint_states.position[43] = 0.0; joint_states.name[44] ="mj5"; joint_states.position[44] = 0.0; joint_states.name[45] ="rij3"; joint_states.position[45] = 0.0; joint_states.name[46] ="rij4"; joint_states.position[46] = 0.0; joint_states.name[47] ="rij5"; joint_states.position[47] = 0.0; joint_states.name[48] ="raj4"; joint_states.position[48] = 0.0; joint_states.name[49] ="raj5"; joint_states.position[49] = 0.0; joint_states.name[50] ="lij3"; joint_states.position[50] = 0.0; joint_states.name[51] ="lij4"; joint_states.position[51] = 0.0; joint_states.name[52] ="lij5"; joint_states.position[52] = 0.0; joint_states.name[53] ="left_wrist_yaw"; joint_states.position[53] = 0.0; joint_states.name[54] ="ltj2"; joint_states.position[54] = 0.0; joint_states.name[55] ="ltj4"; joint_states.position[55] = 0.0; joint_states.name[56] ="ltj5"; joint_states.position[56] = 0.0; joint_states.name[57] ="ltj6"; joint_states.position[57] = 0.0; joint_states.name[58] ="laij3"; joint_states.position[58] = 0.0; joint_states.name[59] ="laij4"; joint_states.position[59] = 0.0; joint_states.name[60] ="laij5"; joint_states.position[60] = 0.0; joint_states.name[61] ="lmj3"; joint_states.position[61] = 0.0; joint_states.name[62] ="lmj4"; joint_states.position[62] = 0.0; joint_states.name[63] ="lmj5"; joint_states.position[63] = 0.0; joint_states.name[64] ="lrij3"; joint_states.position[64] = 0.0; joint_states.name[65] ="lrij4"; joint_states.position[65] = 0.0; joint_states.name[66] ="lrij5"; joint_states.position[66] = 0.0; joint_states.name[67] ="llij3"; joint_states.position[67] = 0.0; joint_states.name[68] ="llij4"; joint_states.position[68] = 0.0; joint_states.name[69] ="llij5"; joint_states.position[69] = 0.0; joint_pub.write(joint_states); while (1) { encsTorso->getEncoders(encodersTorso.data()); encsRArm->getEncoders(encodersRArm.data()); encsLArm->getEncoders(encodersLArm.data()); encsHead->getEncoders(encodersHead.data()); clock_gettime(CLOCK_REALTIME, ¤tTime); joint_states.header.stamp.sec = currentTime.tv_sec; joint_states.header.stamp.nsec = currentTime.tv_nsec; // update torso positions joint_states.position[0] = encodersTorso[2] * degtorad; joint_states.position[1] = encodersTorso[1] * degtorad; joint_states.position[2] = encodersTorso[0] * degtorad; // update head positions joint_states.position[3] = encodersHead[0] * degtorad; joint_states.position[4] = encodersHead[1] * degtorad; joint_states.position[5] = encodersHead[2] * degtorad; // update right arm positions joint_states.position[10] = encodersRArm[0] * degtorad; joint_states.position[11] = encodersRArm[1] * degtorad; joint_states.position[12] = encodersRArm[2] * degtorad; joint_states.position[13] = encodersRArm[3] * degtorad; joint_states.position[14] = encodersRArm[4] * degtorad; joint_states.position[15] = encodersRArm[5] * degtorad; // update left arm positions joint_states.position[16] = encodersLArm[0] * degtorad; joint_states.position[17] = encodersLArm[1] * degtorad; joint_states.position[18] = encodersLArm[2] * degtorad; joint_states.position[19] = encodersLArm[3] * degtorad; joint_states.position[20] = encodersLArm[4] * degtorad; joint_states.position[21] = encodersLArm[5] * degtorad; joint_pub.write(joint_states); Time::delay(0.01); } return 0; }
int main(int argc, char *argv[]) { Network yarp; Property params; params.fromCommand(argc, argv); if (!params.check("robot")) { fprintf(stderr, "Please specify the name of the robot\n"); fprintf(stderr, "--robot name (e.g. icub)\n"); fprintf(stderr, "--part name (right_arm or left_arm)\n"); fprintf(stderr, "--durationInS duration (in seconds)\n"); fprintf(stderr, "--sampleInMS sample time (in milliseconds)\n"); fprintf(stderr, "--refTimeInMS Reference time of minimum jerk trajectory generator (in milliseconds)\n"); return 1; } if( !params.check("sampleInMS") || !params.check("durationInS") || !params.check("refTimeInMS") ) { fprintf(stderr, "Necessary params not passed\n"); return EXIT_FAILURE; } std::string robotName=params.find("robot").asString().c_str(); std::string partName=params.find("part").asString().c_str(); double durationInS = params.find("durationInS").asDouble(); double samplesInMS = params.find("sampleInMS").asDouble(); double refTimeInMS = params.find("refTimeInMS").asDouble(); std::cout << "robotName : " << robotName << std::endl; std::cout << "partName : " << partName << std::endl; std::cout << "durationInS : " << durationInS << std::endl; std::cout << "samplesInMS : " << samplesInMS << std::endl; std::cout << "refTimeInMS : " << refTimeInMS << std::endl; std::string remotePorts="/"; remotePorts+=robotName; remotePorts+="/"+partName; std::string localPorts="/randomShoulderMovements/client"; Property options; options.put("device", "remote_controlboard"); options.put("local", localPorts.c_str()); //local port names options.put("remote", remotePorts.c_str()); //where we connect to // create a device PolyDriver robotDevice(options); if (!robotDevice.isValid()) { printf("Device not available. Here are the known devices:\n"); printf("%s", Drivers::factory().toString().c_str()); return 0; } IPositionDirect *pos; IEncoders *encs; IControlLimits * lims; IControlMode2 * ictrl; bool ok; ok = robotDevice.view(pos); ok = ok && robotDevice.view(encs); ok = ok && robotDevice.view(lims); ok = ok && robotDevice.view(ictrl); if (!ok) { printf("Problems acquiring interfaces\n"); return 0; } int nj=0; pos->getAxes(&nj); Vector encodersInDeg(nj); Vector desPosInDeg(3); Vector commandInDeg(3); Vector minShoulderInDeg(3); Vector maxShoulderInDeg(3); Vector lowerBoundConstraint; Vector upperBoundConstraint; Matrix constraintMatrix; for(int i = 0; i < 3; i++ ) { lims->getLimits(i,&(minShoulderInDeg[i]),&(maxShoulderInDeg[i])); } bool encodersRead = encs->getEncoders(encodersInDeg.data()); while( !encodersRead ) { encodersRead = encs->getEncoders(encodersInDeg.data()); } getShoulderConstraint(lowerBoundConstraint,upperBoundConstraint,constraintMatrix); // Create the minimum jerk filter iCub::ctrl::minJerkTrajGen filter(3,samplesInMS,refTimeInMS); filter.init(encodersInDeg.subVector(0,2)); std::cout << "Filter initial value " << encodersInDeg.subVector(0,2).toString() << std::endl; std::vector<int> indexToControl; indexToControl.push_back(0); indexToControl.push_back(1); indexToControl.push_back(2); // Set control modes ictrl->setControlMode(0,VOCAB_CM_POSITION_DIRECT); ictrl->setControlMode(1,VOCAB_CM_POSITION_DIRECT); ictrl->setControlMode(2,VOCAB_CM_POSITION_DIRECT); double samplesInS = samplesInMS/1000.0; int samples = (int)(durationInS/samplesInS); int samplesForNewPosition = (int)(refTimeInMS/samplesInMS); for(int i=0; i < samples; i++ ) { if( i % samplesForNewPosition == 0 ) { // Generate a new desired shoulder position generateRandomShoulderPosition(desPosInDeg,minShoulderInDeg,maxShoulderInDeg, lowerBoundConstraint,upperBoundConstraint,constraintMatrix); std::cout << "New position generated" << std::endl; std::cout << "Position generated " << desPosInDeg.toString() << std::endl; std::cout << "Remaining time " << (samples-i)*(samplesInS) << std::endl; } filter.computeNextValues(desPosInDeg); commandInDeg = filter.getPos(); std::cout << "Position set " << commandInDeg.toString() << std::endl; pos->setPositions(3,indexToControl.data(),commandInDeg.data()); yarp::os::Time::delay(samplesInS); } robotDevice.close(); return 0; }
int main(int argc, char *argv[]) { Network yarp; Property params; params.fromCommand(argc, argv); if (!params.check("robot")) { fprintf(stderr, "Please specify the name of the robot\n"); fprintf(stderr, "--robot name (e.g. icub)\n"); return -1; } std::string robotName=params.find("robot").asString().c_str(); std::string remotePorts="/"; remotePorts+=robotName; remotePorts+="/right_arm"; std::string localPorts="/test/client"; Property options; options.put("device", "remote_controlboard"); options.put("local", localPorts.c_str()); //local port names options.put("remote", remotePorts.c_str()); //where we connect to // create a device PolyDriver robotDevice(options); if (!robotDevice.isValid()) { printf("Device not available. Here are the known devices:\n"); printf("%s", Drivers::factory().toString().c_str()); return 0; } IPositionControl *pos; IEncoders *encs; bool ok; ok = robotDevice.view(pos); ok = ok && robotDevice.view(encs); if (!ok) { printf("Problems acquiring interfaces\n"); return 0; } int nj=0; pos->getAxes(&nj); Vector encoders; Vector command; Vector tmp; encoders.resize(nj); tmp.resize(nj); command.resize(nj); int i; for (i = 0; i < nj; i++) { tmp[i] = 50.0; } pos->setRefAccelerations(tmp.data()); for (i = 0; i < nj; i++) { tmp[i] = 10.0; pos->setRefSpeed(i, tmp[i]); } //pos->setRefSpeeds(tmp.data())) //fisrst zero all joints // command=0; //now set the shoulder to some value command[0]=50; command[1]=20; command[2]=-10; command[3]=50; pos->positionMove(command.data()); bool done=false; while(!done) { pos->checkMotionDone(&done); Time::delay(0.1); } int times=0; while(true) { times++; if (times%2) { command[0]=50; command[1]=20; command[2]=-10; command[3]=50; // pos->positionMove(command.data()); pos->positionMove(0, command[0]); pos->positionMove(1, command[1]); pos->positionMove(2, command[2]); pos->positionMove(3, command[3]); } else { command[0]=20; command[1]=10; command[2]=-10; command[3]=30; // pos->positionMove(command.data()); pos->positionMove(0, command[0]); pos->positionMove(1, command[1]); pos->positionMove(2, command[2]); pos->positionMove(3, command[3]); } int count=50; while(count--) { Time::delay(0.1); encs->getEncoders(encoders.data()); printf("%.1lf %.1lf %.1lf %.1lf\n", encoders[0], encoders[1], encoders[2], encoders[3]); } } robotDevice.close(); return 0; }
int main(int argc, char *argv[]) { Network yarp; int maxSpeed; Property params; params.fromCommand(argc, argv); if (!params.check("robot")) { fprintf(stderr, "Please specify the name of the robot\n"); fprintf(stderr, "--robot name (e.g. icub)\n"); return -1; } if (!params.check("repetitions")) { fprintf(stderr, "Please specify number of repetitions\n"); fprintf(stderr, "--repetitions num (e.g. 10)\n"); return -1; } if (!params.check("speed")) { fprintf(stderr, "Speed not specified using default\n"); fprintf(stderr, "--speed num (e.g. 2)\n"); maxSpeed = 10.0; } else { maxSpeed = params.find("speed").asInt(); } // sanity check on argument value if(maxSpeed <0 || maxSpeed>50) { maxSpeed = 10; } std::string robotName=params.find("robot").asString().c_str(); std::string remotePorts="/"; remotePorts+=robotName; remotePorts+="/head"; int numTimes = params.find("repetitions").asInt(); std::string localPorts="/headMovement_koroibot/client"; Property options; options.put("device", "remote_controlboard"); options.put("local", localPorts.c_str()); //local port names options.put("remote", remotePorts.c_str()); //where we connect to // create a device PolyDriver robotDevice(options); if (!robotDevice.isValid()) { printf("Device not available. Here are the known devices:\n"); printf("%s", Drivers::factory().toString().c_str()); return 0; } IPositionControl *pos; IEncoders *encs; bool ok; ok = robotDevice.view(pos); ok = ok && robotDevice.view(encs); if (!ok) { printf("Problems acquiring interfaces\n"); return 0; } int nj=0; pos->getAxes(&nj); Vector encoders; Vector command; Vector tmp; encoders.resize(nj); tmp.resize(nj); command.resize(nj); int i; for (i = 0; i < nj; i++) { tmp[i] = 50.0; } pos->setRefAccelerations(tmp.data()); for (i = 0; i < nj; i++) { tmp[i] = 10.0; pos->setRefSpeed(i, tmp[i]); } //first read all encoders printf("waiting for encoders"); while(!encs->getEncoders(encoders.data())) { Time::delay(0.1); printf("."); } printf("\n;"); int ctr =0; bool done = false; while(ctr<numTimes) { printf("Starting headMovement\n"); command=encoders; if(ctr%2 == 0) command[2]=HEAD_YAW_MAX; else command[2]=HEAD_YAW_MIN; done = false; pos->positionMove(command.data()); while(!done) { pos->checkMotionDone(&done); Time::delay(0.1); } ctr++; } command[2] = 0; pos->positionMove(command.data()); while(!done) { pos->checkMotionDone(&done); Time::delay(0.1); } robotDevice.close(); return 0; }
bool partMover::entry_update(partMover *currentPart) { GdkColor color_grey; GdkColor color_yellow; GdkColor color_green; GdkColor color_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_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_blue.red=150*255; color_blue.green=190*255; color_blue.blue=255*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->ctrlmode; IPositionControl *ipos = currentPart->pos; IVelocityControl *ivel = currentPart->iVel; 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]; int k; int NUMBER_OF_JOINTS=0; bool done = false; bool ret = false; ipos->getAxes(&NUMBER_OF_JOINTS); if (NUMBER_OF_JOINTS == 0) { fprintf(stderr,"Lost connection with iCubInterface. You should save and restart.\n" ); Time::delay(0.1); pColor=&color_grey; strcpy(frame_title,"DISCONNECTED"); for (k = 0; k < MAX_NUMBER_OF_JOINTS; k++) { if (currentPart->framesArray[k]!=0) { gtk_frame_set_label (GTK_FRAME(currentPart->framesArray[k]),frame_title); gtk_widget_modify_bg (colorback[k], GTK_STATE_NORMAL, pColor); } } return true; } for (k = 0; k < NUMBER_OF_JOINTS; k++) { max_torques[k]=0; min_torques[k]=0; torques[k]=0; } if (!iiencs->getEncoders(positions)) return true; itrq->getTorques(torques); iiencs->getEncoderSpeeds(speeds); //update all joints positions for (k = 0; k < NUMBER_OF_JOINTS; k++) { sprintf(buffer, "%.1f", positions[k]); gtk_entry_set_text((GtkEntry*) pos_entry[k], buffer); sprintf(buffer, "%.3f", torques[k]); gtk_entry_set_text((GtkEntry*) trq_entry[k], buffer); sprintf(buffer, "%.1f", speeds[k]); gtk_entry_set_text((GtkEntry*) speed_entry[k], buffer); } //update all joint sliders for (k = 0; k < NUMBER_OF_JOINTS; k++) if(POS_UPDATE[k]) gtk_range_set_value((GtkRange*)sliderAry[k], positions[k]); // *** update the checkMotionDone box section *** // (only one at a time in order to save badwidth) k = slowSwitcher%NUMBER_OF_JOINTS; slowSwitcher++; #if DEBUG_GUI gtk_entry_set_text((GtkEntry*) inEntry[k], "off"); #else ipos->checkMotionDone(k, &done); if (!done) gtk_entry_set_text((GtkEntry*) inEntry[k], " "); else gtk_entry_set_text((GtkEntry*) inEntry[k], "@"); #endif // *** update the controlMode section *** // the new icubinterface does not increase the bandwidth consumption // ret = true; useless guys! ret=ictrl->getControlModes(controlModes); if (ret==false) fprintf(stderr,"ictrl->getControlMode failed\n" ); for (k = 0; k < NUMBER_OF_JOINTS; k++) { if (currentPart->first_time==false && controlModes[k] == controlModesOld[k]) continue; controlModesOld[k]=controlModes[k]; sprintf(frame_title,"Joint %d ",k ); switch (controlModes[k]) { case VOCAB_CM_IDLE: pColor=&color_yellow; strcat(frame_title," (IDLE)"); gtk_frame_set_label (GTK_FRAME(currentPart->framesArray[k]),frame_title); gtk_widget_modify_bg (colorback[k], GTK_STATE_NORMAL, pColor); break; case VOCAB_CM_POSITION: pColor=&color_green; strcat(frame_title," (POSITION)"); gtk_frame_set_label (GTK_FRAME(currentPart->framesArray[k]),frame_title); gtk_frame_set_label (GTK_FRAME(currentPart->frame_slider1[k]),"Position:"); gtk_frame_set_label (GTK_FRAME(currentPart->frame_slider2[k]),"Velocity:"); gtk_widget_modify_bg (colorback[k], GTK_STATE_NORMAL, pColor); break; case VOCAB_CM_VELOCITY: pColor=&color_blue; strcat(frame_title," (VELOCITY)"); gtk_frame_set_label (GTK_FRAME(currentPart->framesArray[k]),frame_title); gtk_widget_modify_bg (colorback[k], GTK_STATE_NORMAL, pColor); break; case VOCAB_CM_TORQUE: pColor=&color_pink; strcat(frame_title," (TORQUE)"); gtk_frame_set_label (GTK_FRAME(currentPart->framesArray[k]),frame_title); gtk_frame_set_label (GTK_FRAME(currentPart->frame_slider1[k]),"Torque:"); gtk_frame_set_label (GTK_FRAME(currentPart->frame_slider2[k]),"Torque2:"); gtk_widget_modify_bg (colorback[k], GTK_STATE_NORMAL, pColor); break; case VOCAB_CM_IMPEDANCE_POS: pColor=&color_indaco; strcat(frame_title," (IMPEDANCE POS)"); gtk_frame_set_label (GTK_FRAME(currentPart->framesArray[k]),frame_title); gtk_widget_modify_bg (colorback[k], GTK_STATE_NORMAL, pColor); break; case VOCAB_CM_IMPEDANCE_VEL: pColor=&color_indaco; strcat(frame_title," (IMPEDANCE VEL)"); gtk_frame_set_label (GTK_FRAME(currentPart->framesArray[k]),frame_title); gtk_widget_modify_bg (colorback[k], GTK_STATE_NORMAL, pColor); break; case VOCAB_CM_OPENLOOP: pColor=&color_white; strcat(frame_title," (OPENLOOP)"); gtk_frame_set_label (GTK_FRAME(currentPart->framesArray[k]),frame_title); gtk_widget_modify_bg (colorback[k], GTK_STATE_NORMAL, pColor); break; default: case VOCAB_CM_UNKNOWN: pColor=&color_grey; //strcat(frame_title," (UNKNOWN)"); gtk_frame_set_label (GTK_FRAME(currentPart->framesArray[k]),frame_title); gtk_widget_modify_bg (colorback[k], GTK_STATE_NORMAL, pColor); break; } // pColor=&color_blue; // gtk_widget_modify_bg (colorback[k], GTK_STATE_NORMAL, pColor); int curr_amp_status=0; int amp_status[60]; //fix this!!! for (int i=0; i<60; i++) amp_status[i]=0; //fix this!!! iamp->getAmpStatus(amp_status); //fix this!!! curr_amp_status=amp_status[k]; //fix this!!! #if 0 if ((amp_status[k] & 0xFF)!=0) { //fprintf(stderr, "FAULT DETECTED: %x\n", curr_amp_status); //pColor=&color_red; //strcat(frame_title," (FAULT)"); //gtk_frame_set_label (GTK_FRAME(currentPart->framesArray[k]),frame_title); //gtk_widget_modify_bg (colorback[k], GTK_STATE_NORMAL, pColor); } #endif } currentPart->first_time =false; return true; }
bool exploreTorso(Vector target) { Vector torsoInitialJoints; Vector torsoActualJoints; Vector torsoVelocityCommand; Vector torsoAccCommand; Vector error,integral,derivative,preError; int jointsNumber=0; int i; double time= 0.0; itorsoVelocity->getAxes(&jointsNumber); torsoAccCommand.resize(jointsNumber); torsoVelocityCommand.resize(jointsNumber); torsoInitialJoints.resize(jointsNumber); torsoActualJoints.resize(jointsNumber); error.resize(jointsNumber); integral.resize(jointsNumber); derivative.resize(jointsNumber); preError.resize(jointsNumber); preError[0] = 0.0; preError[1] = 0.0; preError[2] = 0.0; integral[0] = 0.0; integral[1] = 0.0; integral[2] = 0.0; derivative[0] = 0.0; derivative[1] = 0.0; derivative[2] = 0.0; for(i =0; i< jointsNumber;i++); torsoAccCommand[i] = torsoAcceleration[i]; itorsoVelocity->setRefAccelerations(torsoAccCommand.data()); if (!iTorsoEncoder->getEncoders(torsoInitialJoints.data())){ cout<<"Error in reading encoders."<<endl; return false; } VectorOf<int> modes(3); modes[0]=modes[1]=modes[2]=VOCAB_CM_VELOCITY; itorsoMode->setControlModes(modes.getFirst()); error = target-torsoInitialJoints; integral = integral + (error * DT); derivative = (error - preError) / DT; preError = error; torsoVelocityCommand = kp * error + KI * integral + KD * derivative; time = Time::now(); while (norm(error)>0.2){ if(Time::now()-time>maxTorsoTrajTime){ cout<<"Max time reached."<<endl; itorsoVelocity->stop(); return true; } itorsoVelocity->velocityMove(torsoVelocityCommand.data()); if (!iTorsoEncoder->getEncoders(torsoActualJoints.data())){ cout<<"Error in reading encoders."<<endl; itorsoVelocity->stop(); return false; } error = target-torsoActualJoints; integral = integral + (error * DT); derivative = (error - preError) / DT; preError = error; torsoVelocityCommand = kp * error + KI * integral + KD * derivative; Time::delay(DT); } itorsoVelocity->stop(); return true; }