void partMover::go_click(GtkButton *button, partMover *currentPart) { IPositionControl *ipos = currentPart->pos; IEncoders *iiencs = currentPart->iencs; IAmplifierControl *iamp = currentPart->amp; IPidControl *ipid = currentPart->pid; 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 NUMBER_OF_JOINTS; ipos->getAxes(&NUMBER_OF_JOINTS); //get the current row index int i = get_index_selection(currentPart); if (i != -1) { if (TIMING_TMP[i]>0) { ipos->setRefSpeeds(STORED_VEL_TMP[i]); ipos->positionMove(STORED_POS_TMP[i]); for (int k =0; k < NUMBER_OF_JOINTS; k++) { gtk_range_set_value ((GtkRange *) (sliderAry[k]), STORED_POS_TMP[i][k]); gtk_range_set_value ((GtkRange *) (sliderVelAry[k]), STORED_VEL_TMP[i][k]); } } } return; }
void ReachManager::RobotPositionControl(string partName, const Vector &jointAngles) { IPositionControl *pos; IEncoders *encs; if (!(polydrivers[partName]->view(pos) && polydrivers[partName]->view(encs))) { printf("Problems acquiring interfaces\n"); return; } Vector command; command.resize(nbJoints[partName]); //first zero all joints command=0; //now set the shoulder to some value command[0]=jointAngles[0] * 180 / 3.14; command[1]=jointAngles[1] * 180 / 3.14; command[2]=jointAngles[2] * 180 / 3.14; command[3]=jointAngles[3] * 180 / 3.14; command[4]=jointAngles[4] * 180 / 3.14; command[5]=jointAngles[5] * 180 / 3.14; command[6]=jointAngles[6] * 180 / 3.14; cout << "Moving to : " << command.toString() << endl; pos->positionMove(command.data()); return; }
bool goHome() { printMessage(0,"Going home...\n"); yarp::sig::Vector poss(7,0.0); yarp::sig::Vector vels(7,0.0); printMessage(1,"Configuring arm...\n"); poss[0]=-30.0; vels[0]=10.0; poss[1]=30.0; vels[1]=10.0; poss[2]= 00.0; vels[2]=10.0; poss[3]=45.0; vels[3]=10.0; poss[4]= 00.0; vels[4]=10.0; poss[5]=00.0; vels[5]=10.0; poss[6]= 00.0; vels[6]=10.0; for (int i=0; i<7; i++) { iposs->setRefSpeed(i,vels[i]); iposs->positionMove(i,poss[i]); } printMessage(1,"Configuring hand...\n"); poss.resize(9,0.0); vels.resize(9,0.0); poss[0]=40.0; vels[0]=60.0; poss[1]=10.0; vels[1]=60.0; poss[2]=60.0; vels[2]=60.0; poss[3]=70.0; vels[3]=60.0; poss[4]=00.0; vels[4]=60.0; poss[5]=00.0; vels[5]=60.0; poss[6]=70.0; vels[6]=60.0; poss[7]=100.0; vels[7]=60.0; poss[8]=240.0; vels[8]=120.0; for (int i=7; i<nEncs; i++) { iposs->setRefAcceleration(i,1e9); iposs->setRefSpeed(i,vels[i-7]); iposs->positionMove(i,poss[i-7]); } return true; }
void testMotor(PolyDriver& driver) { IPositionControl *pos; if (driver.view(pos)) { int ct = 0; pos->getAxes(&ct); printf(" number of axes is: %d\n", ct); } else { printf(" could not find IPositionControl interface\n"); } }
void partMover::home_all(GtkButton *button, partMover* currentPart) { IPositionControl *ipos = currentPart->pos; IEncoders *iiencs = currentPart->iencs; IAmplifierControl *iamp = currentPart->amp; IPidControl *ipid = currentPart->pid; IControlCalibration2 *ical = currentPart->cal; int NUMBER_OF_JOINTS; ipos->getAxes(&NUMBER_OF_JOINTS); //fprintf(stderr, "Retrieving finder \n"); ResourceFinder *fnd = currentPart->finder; //fprintf(stderr, "Retrieved finder: %p \n", fnd); char buffer1[800]; char buffer2[800]; strcpy(buffer1, currentPart->partLabel); strcpy(buffer2, strcat(buffer1, "_zero")); //fprintf(stderr, "Finder retrieved %s\n", buffer2); if (!fnd->findGroup(buffer2).isNull() && !fnd->isNull()) { bool ok = true; Bottle xtmp, ytmp; xtmp = fnd->findGroup(buffer2).findGroup("PositionZero"); ok = ok && (xtmp.size() == NUMBER_OF_JOINTS+1); ytmp = fnd->findGroup(buffer2).findGroup("VelocityZero"); ok = ok && (ytmp.size() == NUMBER_OF_JOINTS+1); if(ok) { for (int joint = 0; joint < NUMBER_OF_JOINTS; joint++) { double positionZero = xtmp.get(joint+1).asDouble(); //fprintf(stderr, "%f ", positionZero); double velocityZero = ytmp.get(joint+1).asDouble(); //fprintf(stderr, "%f ", velocityZero); ipos->setRefSpeed(joint, velocityZero); ipos->positionMove(joint, positionZero); } } else dialog_message(GTK_MESSAGE_ERROR,(char *) "Check the number of entries in the group", buffer2, true); } else { // currentPart->dialog_message(GTK_MESSAGE_ERROR,"No calib file found", strcat("Define a suitable ", strcat(currentPart->partLabel, "Calib")), true); dialog_message(GTK_MESSAGE_ERROR,(char *) "No zero group found in the supplied file. Define a suitable", buffer2, true); } return; }
void partMover::home_click(GtkButton *button, gtkClassData* currentClassData) { partMover *currentPart = currentClassData->partPointer; int * joint = currentClassData->indexPointer; IPositionControl *ipos = currentPart->pos; IEncoders *iiencs = currentPart->iencs; IAmplifierControl *iamp = currentPart->amp; IPidControl *ipid = currentPart->pid; IControlCalibration2 *ical = currentPart->cal; int NUMBER_OF_JOINTS; ipos->getAxes(&NUMBER_OF_JOINTS); //fprintf(stderr, "Retrieving finder \n"); ResourceFinder *fnd = currentPart->finder; //fprintf(stderr, "Retrieved finder: %p \n", fnd); char buffer1[800]; char buffer2[800]; strcpy(buffer1, currentPart->partLabel); strcpy(buffer2, strcat(buffer1, "_zero")); //fprintf(stderr, "Finder retrieved %s\n", buffer2); if (!fnd->findGroup(buffer2).isNull() && !fnd->isNull()) { //fprintf(stderr, "Home group was not empty \n"); bool ok = true; Bottle xtmp; xtmp = fnd->findGroup(buffer2).findGroup("PositionZero"); ok = ok && (xtmp.size() == NUMBER_OF_JOINTS+1); double positionZero = xtmp.get(*joint+1).asDouble(); //fprintf(stderr, "%f\n", positionZero); xtmp = fnd->findGroup(buffer2).findGroup("VelocityZero"); //fprintf(stderr, "VALUE VEL is %d \n", fnd->findGroup(buffer2).find("VelocityZero").toString().c_str()); ok = ok && (xtmp.size() == NUMBER_OF_JOINTS+1); double velocityZero = xtmp.get(*joint+1).asDouble(); //fprintf(stderr, "%f\n", velocityZero); if(!ok) dialog_message(GTK_MESSAGE_ERROR,(char *) "Check the number of entries in the group", buffer2, true); else { ipos->setRefSpeed(*joint, velocityZero); ipos->positionMove(*joint, positionZero); } } else { // currentPart->dialog_message(GTK_MESSAGE_ERROR,"No calib file found", strcat("Define a suitable ", strcat(currentPart->partLabel, "Calib")), true); dialog_message(GTK_MESSAGE_ERROR,(char *) "No zero group found in the supplied file. Define a suitable", buffer2, true); } return; }
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 partMover::sequence_iterator_time(partMover* currP) { //fprintf(stderr, "calling sequence iterator time \n"); IPositionControl *ipos = currP->pos; IEncoders *iiencs = currP->iencs; IAmplifierControl *iamp = currP->amp; IPidControl *ipid = currP->pid; int *SEQUENCE_TMP = currP->SEQUENCE; double *TIMING_TMP = currP->TIMING; double **STORED_POS_TMP = currP->STORED_POS; double **STORED_VEL_TMP = currP->STORED_VEL; int *INV_SEQUENCE_TMP = currP->INV_SEQUENCE; GtkWidget **sliderAry = currP->sliderArray; GtkWidget **sliderVelAry = currP->sliderVelArray; GtkWidget *tree_view = currP->treeview; guint32* timeout_seqeunce_rate_tmp = currP->timeout_seqeunce_rate; guint* timeout_seqeunce_id_tmp = currP->timeout_seqeunce_id; int *SEQUENCE_ITERATOR_TMP = currP->SEQUENCE_ITERATOR; int j = (*SEQUENCE_ITERATOR_TMP); int NUMBER_OF_JOINTS; ipos->getAxes(&NUMBER_OF_JOINTS); if (INV_SEQUENCE_TMP[j]!=-1) { fixed_time_move(STORED_POS_TMP[INV_SEQUENCE_TMP[j]], TIMING_TMP[j], currP); (*SEQUENCE_ITERATOR_TMP)++; *timeout_seqeunce_rate_tmp = (unsigned int) (TIMING_TMP[j]*1000); gtk_timeout_remove(*timeout_seqeunce_id_tmp); *timeout_seqeunce_id_tmp = gtk_timeout_add(*timeout_seqeunce_rate_tmp, (GtkFunction) sequence_iterator_time, currP); } else { //restart the sequence if finished *SEQUENCE_ITERATOR_TMP = 0; j = 0; fixed_time_move(STORED_POS_TMP[INV_SEQUENCE_TMP[j]], TIMING_TMP[j], currP); (*SEQUENCE_ITERATOR_TMP)++; *timeout_seqeunce_rate_tmp = (unsigned int) (TIMING_TMP[j]*1000); gtk_timeout_remove(*timeout_seqeunce_id_tmp); *timeout_seqeunce_id_tmp = gtk_timeout_add(*timeout_seqeunce_rate_tmp, (GtkFunction) sequence_iterator_time, currP); } return false; }
void partMover::sliderVel_release(GtkRange *range, gtkClassData* currentClassData) { partMover *currentPart = currentClassData->partPointer; int * joint = currentClassData->indexPointer; IPositionControl *ipos = currentPart->pos; GtkWidget **sliderAry = currentPart->sliderArray; double val = gtk_range_get_value(range); double posit = gtk_range_get_value((GtkRange *) sliderAry[*joint]); ipos->setRefSpeed(*joint, val); ipos->positionMove(*joint, posit); return; }
void partMover::slider_release(GtkRange *range, gtkClassData* currentClassData) { partMover *currentPart = currentClassData->partPointer; int * joint = currentClassData->indexPointer; bool *POS_UPDATE = currentPart->CURRENT_POS_UPDATE; IPositionControl *ipos = currentPart->pos; IPidControl *ipid = currentPart->pid; IPositionDirect *iDir = currentPart->iDir; GtkWidget **sliderVel = currentPart->sliderVelArray; double val = gtk_range_get_value(range); double valVel = gtk_range_get_value((GtkRange *)sliderVel[*joint]); IControlMode *iCtrl = currentPart->ctrlmode2; int mode; iCtrl->getControlMode(*joint, &mode); if (!POS_UPDATE[*joint]) { if( ( mode == VOCAB_CM_POSITION) || (mode == VOCAB_CM_MIXED) ) { ipos->setRefSpeed(*joint, valVel); ipos->positionMove(*joint, val); } else if( ( mode == VOCAB_CM_IMPEDANCE_POS)) { fprintf(stderr, " using old 'impedance_position' mode, this control mode is deprecated!"); ipos->setRefSpeed(*joint, valVel); ipos->positionMove(*joint, val); } else if ( mode == VOCAB_CM_POSITION_DIRECT) { if (position_direct_enabled) { iDir->setPosition(*joint, val); } else { fprintf(stderr, "You cannot send direct position commands without using --direct option!"); } } else { fprintf(stderr, "Joint not in position nor positionDirect so cannot send references"); } } return; }
void partMover::sequence_click(GtkButton *button, partMover* currentPart) { IPositionControl *ipos = currentPart->pos; IEncoders *iiencs = currentPart->iencs; IAmplifierControl *iamp = currentPart->amp; IPidControl *ipid = currentPart->pid; 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 j; int NUMBER_OF_JOINTS; ipos->getAxes(&NUMBER_OF_JOINTS); int invSequence[NUMBER_OF_STORED]; for (j = 0; j < NUMBER_OF_STORED; j++) invSequence[j] = -1; for (j = 0; j < NUMBER_OF_STORED; j++) { if (SEQUENCE_TMP[j]>-1 && (SEQUENCE_TMP[j]<NUMBER_OF_STORED)) invSequence[SEQUENCE_TMP[j]] = j; } for (j = 0; j < NUMBER_OF_STORED; j++) if (invSequence[j]!=-1) { if (TIMING_TMP[invSequence[j]] > 0) { ipos->setRefSpeeds(STORED_VEL_TMP[invSequence[j]]); ipos->positionMove(STORED_POS_TMP[invSequence[j]]); for (int k =0; k < NUMBER_OF_JOINTS; k++) { gtk_range_set_value ((GtkRange *) (sliderAry[k]), STORED_POS_TMP[invSequence[j]][k]); gtk_range_set_value ((GtkRange *) (sliderVelAry[k]), STORED_VEL_TMP[invSequence[j]][k]); } Time::delay(TIMING_TMP[invSequence[j]]); } } else break; return; }
void ReachManager::InitPositionControl(string partName) { Property options; options.put("device", "remote_controlboard"); options.put("local", ("/reach_manager/position_control/" + partName).c_str()); //local port names string remotePortName = "/" + (string)parameters["robot"]->asString() + "/" + partName + "_arm"; options.put("remote", remotePortName.c_str()); //where we connect to // create a device polydrivers[partName] = new PolyDriver(options); if (!polydrivers[partName]->isValid()) { printf("Device not available. Here are the known devices:\n"); printf("%s", Drivers::factory().toString().c_str()); return; } IPositionControl *pos; IEncoders *encs; if (!(polydrivers[partName]->view(pos) && polydrivers[partName]->view(encs))) { printf("Problems acquiring interfaces\n"); return; } pos->getAxes(&nbJoints[partName]); Vector encoders; Vector tmp; encoders.resize(nbJoints[partName]); tmp.resize(nbJoints[partName]); for (int i = 0; i < nbJoints[partName]; i++) { tmp[i] = 5.0; } pos->setRefAccelerations(tmp.data()); for (int i = 0; i < nbJoints[partName]; i++) { tmp[i] = 3.0; pos->setRefSpeed(i, tmp[i]); } }
virtual bool threadInit() { // open a client interface to connect to the gaze server // we suppose that: // 1 - the iCub simulator is running; // 2 - the gaze server iKinGazeCtrl is running and // launched with the following options: "--from configSim.ini" Property optGaze("(device gazecontrollerclient)"); optGaze.put("remote","/iKinGazeCtrl"); optGaze.put("local","/gaze_client"); if (!clientGaze.open(optGaze)) return false; // open the view clientGaze.view(igaze); // latch the controller context in order to preserve // it after closing the module // the context contains the tracking mode, the neck limits and so on. igaze->storeContext(&startup_context_id); // set trajectory time: igaze->setNeckTrajTime(0.8); igaze->setEyesTrajTime(0.4); // put the gaze in tracking mode, so that // when the torso moves, the gaze controller // will compensate for it igaze->setTrackingMode(true); // print out some info about the controller Bottle info; igaze->getInfo(info); fprintf(stdout,"info = %s\n",info.toString().c_str()); Property optTorso("(device remote_controlboard)"); optTorso.put("remote","/icubSim/torso"); optTorso.put("local","/torso_client"); if (!clientTorso.open(optTorso)) return false; // open the view clientTorso.view(ienc); clientTorso.view(ipos); ipos->setRefSpeed(0,10.0); fp.resize(3); state=STATE_TRACK; t=t0=t1=t2=t3=t4=Time::now(); return true; }
void partMover::sequence_stop(GtkButton *button,partMover* currP) { //fprintf(stderr, "calling sequence time stop\n"); guint* timeout_seqeunce_id_tmp = currP->timeout_seqeunce_id; GtkWidget **sliderAry = currP->sliderArray; GtkWidget **sliderVelAry = currP->sliderVelArray; IPositionControl *ipos = currP->pos; int NUMBER_OF_JOINTS; ipos->getAxes(&NUMBER_OF_JOINTS); //deactivate all buttons int k; for (k =0; k < NUMBER_OF_JOINTS; k++) { gtk_widget_set_sensitive(sliderVelAry[k], true); gtk_widget_set_sensitive(sliderAry[k], true); } //fprintf(stderr, "Enabling bottons..."); if (currP->button1 != NULL) //fprintf(stderr, "botton is sensitive..."); gtk_widget_set_sensitive(currP->button1, true); //fprintf(stderr, "disabled..."); if (currP->button2 != NULL) gtk_widget_set_sensitive(currP->button2, true); if (currP->button3 != NULL) gtk_widget_set_sensitive(currP->button3, true); if (currP->button4 != NULL) gtk_widget_set_sensitive(currP->button4, true); if (currP->button5 != NULL) gtk_widget_set_sensitive(currP->button5, true); if (currP->button6 != NULL) gtk_widget_set_sensitive(currP->button6, true); if (currP->button7 != NULL) gtk_widget_set_sensitive(currP->button7, true); if (currP->button8 != NULL) gtk_widget_set_sensitive(currP->button8, true); //fprintf(stderr, "done!\n"); gtk_timeout_remove(*timeout_seqeunce_id_tmp); return; }
void partMover::slider_release(GtkRange *range, gtkClassData* currentClassData) { partMover *currentPart = currentClassData->partPointer; int * joint = currentClassData->indexPointer; bool *POS_UPDATE = currentPart->CURRENT_POS_UPDATE; IPositionControl *ipos = currentPart->pos; IPidControl *ipid = currentPart->pid; GtkWidget **sliderVel = currentPart->sliderVelArray; double val = gtk_range_get_value(range); double valVel = gtk_range_get_value((GtkRange *)sliderVel[*joint]); if (!POS_UPDATE[*joint]) { ipos->setRefSpeed(*joint, valVel); ipos->positionMove(*joint, val); //ipid->setReference(*joint, val); } return; }
void partMover::idle_all(GtkButton *button, partMover* currentPart) { IPositionControl *ipos = currentPart->pos; IEncoders *iiencs = currentPart->iencs; IAmplifierControl *iamp = currentPart->amp; IPidControl *ipid = currentPart->pid; GtkWidget **sliderAry = currentPart->sliderArray; double posJoint; int joint; int NUMBER_OF_JOINTS; ipos->getAxes(&NUMBER_OF_JOINTS); for (joint=0; joint < NUMBER_OF_JOINTS; joint++) { iiencs->getEncoder(joint, &posJoint); iamp->disableAmp(joint); ipid->disablePid(joint); gtk_range_set_value ((GtkRange *) (sliderAry[joint]), posJoint); } return; }
void partMover::sequence_time(GtkButton *button, partMover* currentPart) { IPositionControl *ipos = currentPart->pos; int *SEQUENCE_TMP = currentPart->SEQUENCE; double *TIMING_TMP = currentPart->TIMING; double **STORED_POS_TMP = currentPart->STORED_POS; double **STORED_VEL_TMP = currentPart->STORED_VEL; int invSequence[NUMBER_OF_STORED]; int NUMBER_OF_JOINTS; int j; ipos->getAxes(&NUMBER_OF_JOINTS); for (j = 0; j < NUMBER_OF_STORED; j++) invSequence[j] = -1; for (j = 0; j < NUMBER_OF_STORED; j++) { if (SEQUENCE_TMP[j]>-1 && (SEQUENCE_TMP[j]<NUMBER_OF_STORED)) invSequence[SEQUENCE_TMP[j]] = j; } for (j = 0; j < NUMBER_OF_STORED; j++) if (invSequence[j]!=-1) { if (TIMING_TMP[invSequence[j]] > 0) { fixed_time_move(STORED_POS_TMP[invSequence[j]], TIMING_TMP[invSequence[j]], currentPart); Time::delay(TIMING_TMP[invSequence[j]]); } } else break; return; }
bool updateModule() { if (calibrate) { Property options; options.put("finger",fingerName.c_str()); model->calibrate(options); calibrate=false; ipos->setRefAcceleration(joint,1e9); if ((fingerName=="ring")||(fingerName=="little")) ipos->setRefSpeed(joint,60.0); else ipos->setRefSpeed(joint,30.0); ipos->positionMove(joint,*val); } else { if (Node *finger=model->getNode(fingerName)) { Value data; finger->getSensorsData(data); Value out; finger->getOutput(out); fprintf(stdout,"%s sensors data = %s; output = %s\n", finger->getName().c_str(),data.toString().c_str(),out.toString().c_str()); } double fb; ienc->getEncoder(joint,&fb); if (fabs(*val-fb)<5.0) { val==&min?val=&max:val=&min; ipos->positionMove(joint,*val); } } return true; }
// the motion-done callback virtual void gazeEventCallback() { Vector ang; igaze->getAngles(ang); fprintf(stdout,"Actual gaze configuration: (%s) [deg]\n", ang.toString(3,3).c_str()); fprintf(stdout,"Moving the torso; check if the gaze is compensated ...\n"); // move the torso yaw double val; ienc->getEncoder(0,&val); ipos->positionMove(0,val>0.0?-30.0:30.0); t4=t; // detach the callback igaze->unregisterEvent(*this); // switch state state=STATE_STILL; }
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"); } }
bool configure(ResourceFinder &rf) { std::string robotname = rf.check("robot",yarp::os::Value("icubSim")).asString(); Property options; options.put("robot", robotname); // typically from the command line. options.put("device", "remote_controlboard"); string s("/"); s += robotname; s += "/right_arm/keyBabbling"; options.put("local", s.c_str()); s.clear(); s += "/"; s += robotname; s += "/right_arm"; options.put("remote", s.c_str()); if (!dd.open(options)) { cout << "Device not available. Here are the known devices:\n"<< endl; cout << Drivers::factory().toString().c_str() << endl;; return false; } portStreamer.open("/keyboardBabbling/stream:o"); portFromCart.open("/keyboardBabbling/stream:i"); portRpc.open("/keyboarBabbling/rpc"); attach(portRpc); Property optionCart("(device cartesiancontrollerclient)"); optionCart.put("remote", "/" + robotname +"/cartesianController/right_arm"); optionCart.put("local","/keyboardBabbling/cartesian_client/right_arm"); if (!driverCart.open(optionCart)) return false; bool ok; ok = dd.view(pos); ok &= dd.view(vel); ok &= dd.view(pid); ok &= dd.view(amp); ok &= dd.view(armCtrlModeUsed); ok &= dd.view(armImpUsed); ok &= driverCart.view(armCart); ok &= dd.view(ilimRight); iCubFinger* fingerRight = new iCubFinger("right_index"); deque<IControlLimits*> limRight; limRight.push_back(ilimRight); fingerRight->alignJointsBounds(limRight); if (!ok) { cout << "Device not able to acquire views" << endl; dd.close(); return false; } int jnts = 0; pos->getAxes(&jnts); printf("Working with %d axes\n", jnts); vector<bool> mask; mask.resize(jnts); mask[0] = false; mask[1] = false; mask[2] = false; mask[3] = false; mask[4] = true; mask[5] = false; mask[6] = true; mask[7] = false; mask[8] = true; mask[9] = true; mask[10] = true; mask[11] = true; mask[12] = true; mask[13] = true; mask[14] = true; mask[15] = true; cout << "Closing the hand" << endl; // closing the hand: pos->positionMove(4, 30.0); pos->positionMove(6, 10.0); pos->positionMove(8, 10.0); pos->positionMove(9, 17.0); pos->positionMove(10, 170.0); pos->positionMove(11, 0.0); pos->positionMove(12, 0.0); pos->positionMove(13, 90.0); pos->positionMove(14, 180.0); pos->positionMove(15, 180.0); cout << "Waiting to be in initial position..."; bool motionDone=false; while (!motionDone) { motionDone=true; for (int i=0; i<jnts; i++) { if (mask[i]) { bool jntMotionDone = false; pos->checkMotionDone(i, &jntMotionDone); motionDone &= jntMotionDone; } } Time::yield(); // to avoid killing cpu } cout << "ok" << endl; // wait for the hand to be in initial position Matrix R=zeros(3,3); R(0,0)=-1.0; R(1,1)=1; R(2,2)=-1.0; orientation=dcm2axis(R); // enable torso movements as well // in order to enlarge the workspace Vector dof; armCart->getDOF(dof); dof=1.0; dof[1]=0.0; // every dof but the torso roll armCart->setDOF(dof,dof); armCart->setTrajTime(1.0); Vector initPos(3,0.0); if (rf.check("rightHandInitial")) { Bottle *botPos = rf.find("rightHandInitial").asList(); initPos[0] = botPos->get(0).asDouble(); initPos[1] = botPos->get(1).asDouble(); initPos[2] = botPos->get(2).asDouble(); cout<<"Reaching initial position with right hand"<<initPos.toString(3,3).c_str()<<endl; armCart->goToPoseSync(initPos,orientation); armCart->waitMotionDone(0.1,3.0); } else { cout << "Cannot find the inital position" << endl << "closing module" << endl; return false; } minY = rf.find("min").asDouble(); maxY = rf.find("max").asDouble(); gap = rf.find("gap").asDouble(); delay = rf.find("delay").asDouble(); thrMove = rf.find("threshold_move").asDouble(); yDivisions = rf.check("yDivisions",Value(10)).asInt(); Bottle* bsequenceToPlay = rf.find("sequence").asList(); if (bsequenceToPlay) { cout << "Using sequence, not random." << endl; for (int i = 0; i < bsequenceToPlay->size(); i++) sequenceToPlay.push_back(bsequenceToPlay->get(i).asInt()); indexInSequence = 0; } tempPos=initPos; state=up; forward = false; timeBeginIdle = Time::now(); Rand::init(); return true; }
int main(int argc, char *argv[]){ Network yarp; Port armPlan; Port armPred; BufferedPort<Vector> objAngles; BufferedPort<Vector> armOut; armPlan.open("/learnedReach/plan"); armPred.open("/learnedReach/pred"); objAngles.open("/learnedReach/loc:i"); armOut.open("/learnedReach/arm:o"); bool fwCvOn = 0; fwCvOn = Network::connect("/learnedReach/plan","/fwdConv:i"); fwCvOn *= Network::connect("/fwdConv:o","/learnedReach/pred"); if (!fwCvOn){ printf("Please run command:\n ./fwdConv --input /fwdConv:i --output /fwdConv:o\n"); return -1; } Property params; params.fromCommand(argc,argv); if (!params.check("robot")){ fprintf(stderr, "Please specify robot name\n"); fprintf(stderr, "e.g. --robot icub\n"); return -1; } std::string robotName = params.find("robot").asString().c_str(); std::string remotePorts = "/"; remotePorts += robotName; remotePorts += "/"; if (params.check("arm")){ remotePorts += params.find("arm").asString().c_str(); } else{ remotePorts += "left"; } remotePorts += "_arm"; std::string localPorts = "/learnedReach/cmd"; if(!params.check("map")){ fprintf(stderr, "Please specify learned visuomotor map file\n"); fprintf(stderr, "e.g. --map map.dat\n"); return -1; } string fName = params.find("map").asString().c_str(); Property options; options.put("device", "remote_controlboard"); options.put("local", localPorts.c_str()); options.put("remote", remotePorts.c_str()); PolyDriver robotDevice(options); if (!robotDevice.isValid()){ printf("Device not available. Here are known devices: \n"); printf("%s", Drivers::factory().toString().c_str()); Network::fini(); return 1; } IPositionControl *pos; IEncoders *enc; bool ok; ok = robotDevice.view(pos); ok = ok && robotDevice.view(enc); if (!ok){ printf("Problems acquiring interfaces\n"); return 0; } int nj = 0; pos->getAxes(&nj); Vector encoders; Vector command; Vector commandCart; Vector tmp; encoders.resize(nj); tmp.resize(nj); command.resize(nj); commandCart.resize(3); for (int i = 0; i < nj; i++) { tmp[i] = 25.0; } pos->setRefAccelerations(tmp.data()); for (int i = 0; i < nj; i++) { tmp[i] = 20.0; pos->setRefSpeed(i, tmp[i]); } command = 0; //set the arm joints to "middle" values command[0] = -45; command[1] = 45; command[2] = 0; command[3] = 45; pos->positionMove(command.data()); bool done = false; while (!done){ pos->checkMotionDone(&done); Time::delay(0.1); } //not really yaw and pitch int azMin = -80; int azMax = 0; int elMin = -60; int elMax = 0; int verMin = 0; int verMax = 20; int Y; int P; int V; int mmapSize; int usedJoints; //read in first lines to get map dimensions string line; string buf; ifstream mapFile(fName.c_str()); if(mapFile.is_open()){ getline(mapFile,line); stringstream ss(line); ss >> buf; Y = atoi(buf.c_str()); ss >> buf; P = atoi(buf.c_str()); ss >> buf; V = atoi(buf.c_str()); ss.clear(); getline(mapFile,line); ss.str(line); ss >> buf; mmapSize = atoi(buf.c_str()); ss >> buf; usedJoints = atoi(buf.c_str()); }
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(); } } }
virtual bool threadInit() { /* Read in parameters from resource finder */ loadParams(); /* Start up gaze control client interface */ Property option("(device gazecontrollerclient)"); option.put("remote","/iKinGazeCtrl"); option.put("local","/client/gaze"); clientGazeCtrl.open(option); gaze=NULL; if (clientGazeCtrl.isValid()) { clientGazeCtrl.view(gaze); } else { printf("could not initialize gaze control interface, failing...\n"); return false; } //set gaze control interface params gaze->setNeckTrajTime(neckTT); gaze->setEyesTrajTime(eyeTT); gaze->bindNeckPitch(-30,30); gaze->bindNeckYaw(-25,25); gaze->bindNeckRoll(-10,10); /* Start up arm cartesian control client interface */ Property optiona("(device cartesiancontrollerclient)"); if (armInUse) { string tmpcrname = "/" + robotname + "/cartesianController/right_arm"; optiona.put("remote",tmpcrname.c_str()); optiona.put("local","/cartesian_client/right_arm"); } else { string tmpcrname = "/" + robotname + "/cartesianController/left_arm"; optiona.put("remote",tmpcrname.c_str()); optiona.put("local","/cartesian_client/left_arm"); } clientArmCart.open(optiona); carm = NULL; if (clientArmCart.isValid()) { clientArmCart.view(carm); } else { printf("could not initialize arm cartesian control interface, failing...\n"); return false; } // set trajectory time carm->setTrajTime(trajTime); //slow for safety // get the torso dofs Vector newDof, curDof; carm->getDOF(curDof); newDof=curDof; //enable torso pitch and yaw, also wrist movements newDof[0]=1; newDof[1]=0; newDof[2]=1; newDof[7]=1; newDof[8]=1; newDof[9]=1; // impose some restriction on the torso pitch double tpmin, tpmax; carm->getLimits(0,&tpmin,&tpmax); carm->setLimits(0,tpmin,maxPitch); // send the request for dofs reconfiguration carm->setDOF(newDof,curDof); string lname, rname; Property doption; doption.put("device", "remote_controlboard"); lname = "/"+name+"/torso"; rname = "/"+robotname+"/torso"; doption.put("local", lname.c_str()); doption.put("remote", rname.c_str()); robotTorso.open(doption); if (!robotTorso.isValid()) { printf("could not initialize torso control interface, failing...\n"); return false; } robotTorso.view(tPos); robotTorso.view(tEnc); lname = "/"+name+"/head"; rname = "/"+robotname+"/head"; doption.put("local", lname.c_str()); doption.put("remote", rname.c_str()); robotHead.open(doption); if (!robotHead.isValid()) { printf("could not initialize head control interface, failing...\n"); return false; } robotHead.view(hPos); robotHead.view(hEnc); lname = "/"+name+"/"+armname+"_arm"; rname = "/"+robotname+"/"+armname+"_arm"; doption.put("local", lname.c_str()); doption.put("remote", rname.c_str()); robotArm.open(doption); if (!robotArm.isValid()) { printf("could not initialize arm control interface, failing...\n"); return false; } robotArm.view(aPos); robotArm.view(aEnc); for (int i=0; i<7; i++) { aPos->setRefSpeed(i, 10.0); } /* Create and open ports */ cportL=new ClickPort(bfL); string cplName="/"+name+"/clk:l"; cportL->open(cplName.c_str()); cportL->useCallback(); cportR=new ClickPort(bfR); string cprName="/"+name+"/clk:r"; cportR->open(cprName.c_str()); cportR->useCallback(); susPort=new Port; string suspName="/"+name+"/sus:o"; susPort->open(suspName.c_str()); 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; }
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 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; }
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; }
void partMover::calib_click(GtkButton *button, gtkClassData* currentClassData) { //ask for confirmation if (!dialog_question("Do you really want to recalibrate the joint?")) { return; } partMover *currentPart = currentClassData->partPointer; int * joint = currentClassData->indexPointer; IPositionControl *ipos = currentPart->pos; IEncoders *iiencs = currentPart->iencs; IAmplifierControl *iamp = currentPart->amp; IPidControl *ipid = currentPart->pid; IControlCalibration2 *ical = currentPart->cal; int NUMBER_OF_JOINTS; ipos->getAxes(&NUMBER_OF_JOINTS); ResourceFinder *fnd = currentPart->finder; //fprintf(stderr, "opening file \n"); char buffer1[800]; char buffer2[800]; strcpy(buffer1, currentPart->partLabel); strcpy(buffer2, strcat(buffer1, "_calib")); if (!fnd->findGroup(buffer2).isNull()) { bool ok = true; Bottle xtmp; xtmp.clear(); xtmp = fnd->findGroup(buffer2).findGroup("CalibrationType"); ok = ok && (xtmp.size() == NUMBER_OF_JOINTS+1); unsigned char type = (unsigned char) xtmp.get(*joint+1).asDouble(); fprintf(stderr, "%d ", type); xtmp.clear(); xtmp = fnd->findGroup(buffer2).findGroup("Calibration1"); ok = ok && (xtmp.size() == NUMBER_OF_JOINTS+1); double param1 = xtmp.get(*joint+1).asDouble(); fprintf(stderr, "%f ", param1); xtmp.clear(); xtmp = fnd->findGroup(buffer2).findGroup("Calibration2"); ok = ok && (xtmp.size() == NUMBER_OF_JOINTS+1); double param2 = xtmp.get(*joint+1).asDouble(); fprintf(stderr, "%f ", param2); xtmp.clear(); xtmp = fnd->findGroup(buffer2).findGroup("Calibration3"); ok = ok && (xtmp.size() == NUMBER_OF_JOINTS+1); double param3 = xtmp.get(*joint+1).asDouble(); fprintf(stderr, "%f \n", param3); if(!ok) dialog_message(GTK_MESSAGE_ERROR,(char *)"Check number of calib entries in the group", buffer2, true); else ical->calibrate2(*joint, type, param1, param2, param3); } else dialog_message(GTK_MESSAGE_ERROR,(char *)"The supplied file does not conatain a group named:", buffer2, true); return; }
virtual bool threadInit(){ if(!handleParams()){ return false; } armPlan = new Port; armPred = new Port; armLocJ = new BufferedPort<Vector>; headLoc = new BufferedPort<Vector>; armPlan->open("/babbleTrack/plan:o"); armPred->open("/babbleTrack/pred:i"); armLocJ->open("/babbleTrack/arm:o"); headLoc->open("/babbleTrack/head:o"); gsl_rng_env_setup(); T = gsl_rng_default; r = gsl_rng_alloc(T); igaze = NULL; Property options; options.put("device","gazecontrollerclient"); options.put("remote","/iKinGazeCtrl"); options.put("local","/client/gaze"); clientGazeCtrl = new PolyDriver; clientGazeCtrl->open(options); options.clear(); string localPorts = "/babbleTrack/cmd"; string remotePorts = "/" + robotName + "/" + arm + "_arm"; options.put("device", "remote_controlboard"); options.put("local", localPorts.c_str()); options.put("remote", remotePorts.c_str()); robotDevice = new PolyDriver; robotDevice->open(options); if(clientGazeCtrl->isValid()){ clientGazeCtrl->view(igaze); } else{ return false; } if (!robotDevice->isValid()){ printf("Device not available. Here are known devices: \n"); printf("%s", Drivers::factory().toString().c_str()); Network::fini(); return false; } bool ok; ok = robotDevice->view(pos); ok = ok && robotDevice->view(enc); if (!ok){ printf("Problems acquiring interfaces\n"); return false; } pos->getAxes(&nj); command = new Vector; tmp = new Vector; command->resize(nj); tmp->resize(nj); for (int i = 0; i < nj; i++) { (*tmp)[i] = 25.0; } pos->setRefAccelerations(tmp->data()); for (int i = 0; i < nj; i++) { (*tmp)[i] = 20.0; pos->setRefSpeed(i, (*tmp)[i]); } *command = 0; //set the arm joints to "middle" values (*command)[0] = -45; (*command)[1] = 45; (*command)[2] = 0; (*command)[3] = 45; //flex hand (*command)[4] = 60; (*command)[7] = 20; (*command)[10] = 15; (*command)[11] = 15; (*command)[12] = 15; (*command)[13] = 15; (*command)[14] = 15; (*command)[15] = 15; pos->positionMove(command->data()); bool done = false; while (!done){ pos->checkMotionDone(&done); Time::delay(0.1); } bool fwCvOn = 0; fwCvOn = Network::connect("/babbleTrack/plan:o","/fwdConv:i"); fwCvOn *= Network::connect("/fwdConv:o","/babbleTrack/pred:i"); if (!fwCvOn){ printf("Please run command:\n ./fwdConv --input /fwdConv:i --output /fwdConv:o\n"); return false; } return true; }