/* * Disable PID */ void partMover::dis_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; IControlMode2 *ictrl = currentPart->ctrlmode2; #if 0 ictrl->setControlMode(*joint,VOCAB_CM_IDLE); #else ictrl->setControlMode(*joint,VOCAB_CM_FORCE_IDLE); #endif return; }
void idlingCoupledJoints(const int i, const bool sw) { IControlMode2 *imod; driver->view(imod); PidData &pid=pids[i]; for (size_t j=0; j<pid.idling_joints.size(); j++) imod->setControlMode(pid.idling_joints[j], sw?VOCAB_CM_IDLE:VOCAB_CM_POSITION); }
bool close() { // Terminate model Controller_terminate(Controller_M); imod->setControlMode(joint,VOCAB_CM_POSITION); if (compliant) iint->setInteractionMode(joint,VOCAB_IM_STIFF); rpc.close(); dataOut.close(); dataIn.close(); drv.close(); return true; }
void partMover::run_click(GtkButton *button, gtkClassData* currentClassData) { partMover *currentPart = currentClassData->partPointer; int * joint = currentClassData->indexPointer; IPositionControl *ipos = currentPart->pos; IEncoders *iiencs = currentPart->iencs; IAmplifierControl *iamp = currentPart->amp; IPidControl *ipid = currentPart->pid; IControlMode2 *ictrl = currentPart->ctrlmode2; GtkWidget **sliderAry = currentPart->sliderArray; double posJoint; while (!iiencs->getEncoder(*joint, &posJoint)) Time::delay(0.001); ictrl->setControlMode(*joint,VOCAB_CM_POSITION); gtk_range_set_value ((GtkRange *) (sliderAry[*joint]), posJoint); return; }
void SpringyFingersModel::calibrateFinger(SpringyFinger &finger, const int joint, const double min, const double max) { printMessage(1,"calibrating finger %s ...\n",finger.getName().c_str()); double margin=0.1*(max-min); double _min=min+margin; double _max=max-margin; double tol_min=5.0; double tol_max=5.0; double *val=&_min; double *tol=&tol_min; double timeout=2.0*(_max-_min)/finger.getCalibVel(); mutex.lock(); IControlMode2 *imod; driver.view(imod); IEncoders *ienc; driver.view(ienc); IPositionControl *ipos; driver.view(ipos); mutex.unlock(); // workaround if ((finger.getName()=="ring") || (finger.getName()=="little")) { _min=30.0; _max=180.0; tol_min=20.0; tol_max=50.0; } Property reset("(reset)"); Property feed("(feed)"); Property train("(train)"); finger.calibrate(reset); mutex.lock(); imod->setControlMode(joint,VOCAB_CM_POSITION); ipos->setRefSpeed(joint,finger.getCalibVel()); mutex.unlock(); for (int i=0; i<5; i++) { mutex.lock(); ipos->positionMove(joint,*val); mutex.unlock(); bool done=false; double fbOld=1e9; double t0=Time::now(); while (!done) { Time::delay(0.01); mutex.lock(); double fb; ienc->getEncoder(joint,&fb); mutex.unlock(); if (fabs(fb-fbOld)>0.5) { printMessage(2,"feeding finger %s\n",finger.getName().c_str()); finger.calibrate(feed); } done=(fabs(*val-fb)<*tol)||(Time::now()-t0>timeout); fbOld=fb; } if (val==&_min) { val=&_max; tol=&tol_max; } else { val=&_min; tol=&tol_min; } } printMessage(1,"training finger %s ...\n",finger.getName().c_str()); finger.calibrate(train); printMessage(1,"finger %s trained!\n",finger.getName().c_str()); }
bool SpringyFingersModel::calibrate(const Property &options) { if (configured) { IControlMode2 *imod; driver.view(imod); IControlLimits *ilim; driver.view(ilim); IEncoders *ienc; driver.view(ienc); IPositionControl *ipos; driver.view(ipos); int nAxes; ienc->getAxes(&nAxes); Vector qmin(nAxes),qmax(nAxes),vel(nAxes),acc(nAxes); printMessage(1,"steering the hand to a suitable starting configuration\n"); for (int j=7; j<nAxes; j++) { imod->setControlMode(j,VOCAB_CM_POSITION); ilim->getLimits(j,&qmin[j],&qmax[j]); ipos->getRefAcceleration(j,&acc[j]); ipos->getRefSpeed(j,&vel[j]); ipos->setRefAcceleration(j,1e9); ipos->setRefSpeed(j,60.0); ipos->positionMove(j,(j==8)?qmax[j]:qmin[j]); // thumb in opposition } printMessage(1,"proceeding with the calibration\n"); Property &opt=const_cast<Property&>(options); string tag=opt.check("finger",Value("all")).asString().c_str(); if (tag=="thumb") { calibrateFinger(fingers[0],10,qmin[10],qmax[10]); } else if (tag=="index") { calibrateFinger(fingers[1],12,qmin[12],qmax[12]); } else if (tag=="middle") { calibrateFinger(fingers[2],14,qmin[14],qmax[14]); } else if (tag=="ring") { calibrateFinger(fingers[3],15,qmin[15],qmax[15]); } else if (tag=="little") { calibrateFinger(fingers[4],15,qmin[15],qmax[15]); } else if ((tag=="all") || (tag=="all_serial")) { calibrateFinger(fingers[0],10,qmin[10],qmax[10]); calibrateFinger(fingers[1],12,qmin[12],qmax[12]); calibrateFinger(fingers[2],14,qmin[14],qmax[14]); calibrateFinger(fingers[3],15,qmin[15],qmax[15]); calibrateFinger(fingers[4],15,qmin[15],qmax[15]); } else if (tag=="all_parallel") { CalibThread thr[5]; thr[0].setInfo(this,fingers[0],10,qmin[10],qmax[10]); thr[1].setInfo(this,fingers[1],12,qmin[12],qmax[12]); thr[2].setInfo(this,fingers[2],14,qmin[14],qmax[14]); thr[3].setInfo(this,fingers[3],15,qmin[15],qmax[15]); thr[4].setInfo(this,fingers[4],15,qmin[15],qmax[15]); thr[0].start(); thr[1].start(); thr[2].start(); thr[3].start(); thr[4].start(); bool done=false; while (!done) { done=true; for (int i=0; i<5; i++) { done&=thr[i].isDone(); if (thr[i].isDone() && thr[i].isRunning()) thr[i].stop(); } Time::delay(0.1); } } else { printMessage(1,"unknown finger request %s\n",tag.c_str()); return false; } for (int j=7; j<nAxes; j++) { ipos->setRefAcceleration(j,acc[j]); ipos->setRefSpeed(j,vel[j]); } return true; } else return false; }
bool tune(const int i) { PidData &pid=pids[i]; Property pGeneral; pGeneral.put("joint",i); string sGeneral="(general "; sGeneral+=pGeneral.toString().c_str(); sGeneral+=')'; Bottle bGeneral,bPlantEstimation,bStictionEstimation; bGeneral.fromString(sGeneral.c_str()); bPlantEstimation.fromString("(plant_estimation (Ts 0.01) (Q 1.0) (R 1.0) (P0 100000.0) (tau 1.0) (K 1.0) (max_pwm 800.0))"); bStictionEstimation.fromString("(stiction_estimation (Ts 0.01) (T 2.0) (vel_thres 5.0) (e_thres 1.0) (gamma (10.0 10.0)) (stiction (0.0 0.0)))"); Bottle bConf=bGeneral; bConf.append(bPlantEstimation); bConf.append(bStictionEstimation); Property pOptions(bConf.toString().c_str()); OnlineCompensatorDesign designer; if (!designer.configure(*driver,pOptions)) { yError("designer configuration failed!"); return false; } idlingCoupledJoints(i,true); Property pPlantEstimation; pPlantEstimation.put("max_time",20.0); pPlantEstimation.put("switch_timeout",2.0); designer.startPlantEstimation(pPlantEstimation); yInfo("Estimating plant for joint %d: max duration = %g seconds", i,pPlantEstimation.find("max_time").asDouble()); double t0=Time::now(); while (!designer.isDone()) { yInfo("elapsed %d [s]",(int)(Time::now()-t0)); Time::delay(1.0); if (interrupting) { idlingCoupledJoints(i,false); return false; } } Property pResults; designer.getResults(pResults); double tau=pResults.find("tau_mean").asDouble(); double K=pResults.find("K_mean").asDouble(); yInfo("plant = %g/s * 1/(1+s*%g)",K,tau); Property pControllerRequirements,pController; pControllerRequirements.put("tau",tau); pControllerRequirements.put("K",K); pControllerRequirements.put("f_c",0.75); if (i!=15) { pControllerRequirements.put("T_dr",1.0); pControllerRequirements.put("type","PI"); } else pControllerRequirements.put("type","P"); designer.tuneController(pControllerRequirements,pController); yInfo("tuning results: %s",pController.toString().c_str()); double Kp=pController.find("Kp").asDouble(); double Ki=pController.find("Ki").asDouble(); pid.scale=4.0; int scale=(int)pid.scale; int shift=1<<scale; double fwKp=floor(Kp*pid.encs_ratio*shift); double fwKi=floor(Ki*pid.encs_ratio*shift/1000.0); pid.Kp=yarp::math::sign(pid.Kp*fwKp)>0.0?fwKp:-fwKp; pid.Ki=yarp::math::sign(pid.Ki*fwKi)>0.0?fwKi:-fwKi; pid.Kd=0.0; yInfo("Kp (FW) = %g; Ki (FW) = %g; Kd (FW) = %g; shift factor = %d",pid.Kp,pid.Ki,pid.Kd,scale); Property pStictionEstimation; pStictionEstimation.put("max_time",60.0); pStictionEstimation.put("Kp",Kp); pStictionEstimation.put("Ki",0.0); pStictionEstimation.put("Kd",0.0); designer.startStictionEstimation(pStictionEstimation); yInfo("Estimating stiction for joint %d: max duration = %g seconds", i,pStictionEstimation.find("max_time").asDouble()); t0=Time::now(); while (!designer.isDone()) { yInfo("elapsed %d [s]",(int)(Time::now()-t0)); Time::delay(1.0); if (interrupting) { idlingCoupledJoints(i,false); return false; } } designer.getResults(pResults); pid.st_up=floor(pResults.find("stiction").asList()->get(0).asDouble()); pid.st_down=floor(pResults.find("stiction").asList()->get(1).asDouble()); yInfo("Stiction values: up = %g; down = %g",pid.st_up,pid.st_down); IControlMode2 *imod; IPositionControl *ipos; IEncoders *ienc; driver->view(imod); driver->view(ipos); driver->view(ienc); imod->setControlMode(i,VOCAB_CM_POSITION); ipos->setRefSpeed(i,50.0); ipos->positionMove(i,0.0); yInfo("Driving the joint back to rest... "); t0=Time::now(); while (Time::now()-t0<5.0) { double enc; ienc->getEncoder(i,&enc); if (fabs(enc)<1.0) break; if (interrupting) { idlingCoupledJoints(i,false); return false; } Time::delay(0.2); } yInfo("done!"); idlingCoupledJoints(i,false); return true; }
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; }
bool configure(ResourceFinder &rf) { Time::turboBoost(); string name=rf.check("name",Value("yarpMinJerk")).asString().c_str(); string robot=rf.check("robot",Value("icubSim")).asString().c_str(); string part=rf.check("part",Value("left_arm")).asString().c_str(); joint=rf.check("joint",Value(3)).asInt(); compliant=rf.check("compliant"); Property option; option.put("device","remote_controlboard"); option.put("remote",("/"+robot+"/"+part).c_str()); option.put("local",("/"+name+"/"+part).c_str()); if (!drv.open(option)) return false; drv.view(imod); drv.view(iint); drv.view(ienc); drv.view(ivel); imod->setControlMode(joint,VOCAB_CM_VELOCITY); if (compliant) iint->setInteractionMode(joint,VOCAB_IM_COMPLIANT); IControlLimits *ilim; drv.view(ilim); double min_joint,max_joint; ilim->getLimits(joint,&min_joint,&max_joint); double enc; while (!ienc->getEncoder(joint,&enc)) Time::delay(0.1); Controller_P.Plant_IC=enc; Controller_P.Plant_Max=max_joint; Controller_P.Plant_Min=min_joint; Controller_P.AutoCompensator_ThresHystMax=0.5; Controller_P.AutoCompensator_ThresHystMin=0.1; yInfo("enc=%g in [%g, %g] deg",enc,min_joint,max_joint); // Pack model data into RTM Controller_M->ModelData.defaultParam = &Controller_P; Controller_M->ModelData.blockIO = &Controller_B; Controller_M->ModelData.dwork = &Controller_DW; // Initialize model Controller_initialize(Controller_M, &Controller_U_reference, &Controller_U_compensator_state, &Controller_U_plant_output, &Controller_Y_controller_output, &Controller_Y_controller_reference, &Controller_Y_plant_reference, &Controller_Y_error_statistics, &Controller_Y_enable_compensation); Controller_U_reference=enc; Controller_U_compensator_state=CompensatorState::Off; Controller_U_plant_output=enc; dataIn.open(("/"+name+"/data:i").c_str()); dataOut.open(("/"+name+"/data:o").c_str()); rpc.open(("/"+name+"/rpc").c_str()); attach(rpc); return true; }