iCub-main
springyFingers.cpp
Go to the documentation of this file.
1 /*
2  * Copyright (C) 2011 Department of Robotics Brain and Cognitive Sciences - Istituto Italiano di Tecnologia
3  * Author: Ugo Pattacini
4  * email: ugo.pattacini@iit.it
5  * Permission is granted to copy, distribute, and/or modify this program
6  * under the terms of the GNU General Public License, version 2 or any
7  * later version published by the Free Software Foundation.
8  *
9  * A copy of the license can be found at
10  * http://www.robotcub.org/icub/license/gpl.txt
11  *
12  * This program is distributed in the hope that it will be useful, but
13  * WITHOUT ANY WARRANTY; without even the implied warranty of
14  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General
15  * Public License for more details
16 */
17 
18 #include <sstream>
19 #include <iomanip>
20 #include <cmath>
21 #include <limits>
22 #include <deque>
23 #include <set>
24 
25 #include <yarp/math/Math.h>
26 
27 #include <iCub/ctrl/math.h>
31 
32 using namespace std;
33 using namespace yarp::os;
34 using namespace yarp::dev;
35 using namespace yarp::sig;
36 using namespace yarp::math;
37 using namespace iCub::ctrl;
38 using namespace iCub::learningmachine;
39 using namespace iCub::perception;
40 
41 
42 /************************************************************************/
43 bool SpringyFinger::fromProperty(const Property &options)
44 {
45  if (!options.check("name"))
46  return false;
47 
48  sensors.clear();
49  callbacks.clear();
50  neighbors.clear();
51  lssvm.reset();
52 
53  name=options.find("name").asString();
54 
55  scaler.setLowerBoundIn(0.0);
56  scaler.setUpperBoundIn(255.0);
57  scaler.setLowerBoundOut(0.0);
58  scaler.setUpperBoundOut(1.0);
59 
60  lssvm.setDomainSize(1);
61  lssvm.setC(1e4);
62  lssvm.getKernel()->setGamma(500.0);
63 
64  double defaultCalibVel;
65  if ((name=="thumb") || (name=="index") || (name=="middle"))
66  {
67  lssvm.setCoDomainSize(2);
68  defaultCalibVel=30.0;
69  }
70  else if ((name=="ring") || (name=="little"))
71  {
72  lssvm.setCoDomainSize(3);
73  defaultCalibVel=60.0;
74  }
75  else
76  return false;
77 
78  calibratingVelocity=options.check("calib_vel",Value(defaultCalibVel)).asFloat64();
79  outputGain=options.check("output_gain",Value(1.0)).asFloat64();
80  calibrated=(options.check("calibrated",Value("false")).asString()=="true");
81 
82  if (options.check("scaler"))
83  {
84  Bottle *pB=options.find("scaler").asList();
85  scaler.fromString(pB->toString());
86  }
87 
88  if (options.check("lssvm"))
89  {
90  Bottle *pB=options.find("lssvm").asList();
91  lssvm.fromString(pB->toString());
92  }
93 
94  return true;
95 }
96 
97 
98 /************************************************************************/
99 void SpringyFinger::toProperty(Property &options) const
100 {
101  options.clear();
102  options.put("name",name);
103  options.put("calib_vel",calibratingVelocity);
104  options.put("output_gain",outputGain);
105  options.put("calibrated",calibrated?"true":"false");
106  options.put("scaler","("+scaler.toString()+")");
107  options.put("lssvm","("+lssvm.toString()+")");
108 }
109 
110 
111 /************************************************************************/
112 bool SpringyFinger::toStream(ostream &str) const
113 {
114  str<<"name "<<name<<endl;
115  str<<"calib_vel "<<calibratingVelocity<<endl;
116  str<<"output_gain "<<outputGain<<endl;
117  str<<"calibrated "<<(calibrated?"true":"false")<<endl;
118  str<<"scaler "<<"("+scaler.toString()+")"<<endl;
119  str<<"lssvm "<<"("+lssvm.toString()+")"<<endl;
120 
121  return !str.fail();
122 }
123 
124 
125 /************************************************************************/
126 bool SpringyFinger::getSensorsData(Value &data) const
127 {
128  map<string,Sensor*>::const_iterator In_0=sensors.find("In_0");
129  map<string,Sensor*>::const_iterator Out_0=sensors.find("Out_0");
130  map<string,Sensor*>::const_iterator Out_1=sensors.find("Out_1");
131  map<string,Sensor*>::const_iterator Out_2=sensors.find("Out_2");
132 
133  bool ok=true;
134  ok&=In_0!=sensors.end();
135  ok&=Out_0!=sensors.end();
136  ok&=Out_1!=sensors.end();
137 
138  if (lssvm.getCoDomainSize()>2)
139  ok&=Out_2!=sensors.end();
140 
141  if (!ok)
142  return false;
143 
144  Value val_in, val_out[3];
145  In_0->second->getOutput(val_in);
146  Out_0->second->getOutput(val_out[0]);
147  Out_1->second->getOutput(val_out[1]);
148 
149  Vector in(lssvm.getDomainSize());
150  in[0]=val_in.asFloat64();
151 
152  Vector out(lssvm.getCoDomainSize());
153  out[0]=val_out[0].asFloat64();
154  out[1]=val_out[1].asFloat64();
155 
156  if (lssvm.getCoDomainSize()>2)
157  {
158  Out_2->second->getOutput(val_out[2]);
159  out[2]=val_out[2].asFloat64();
160  }
161 
162  Property prop;
163  Bottle b;
164 
165  b.addList().read(in);
166  prop.put("in",b.get(0));
167 
168  b.addList().read(out);
169  prop.put("out",b.get(1));
170 
171  b.addList().read(prop);
172  data=b.get(2);
173 
174  return true;
175 }
176 
177 
178 /************************************************************************/
179 bool SpringyFinger::extractSensorsData(Vector &in, Vector &out) const
180 {
181  bool ret=false;
182 
183  Value data;
184  if (getSensorsData(data))
185  {
186  if (Bottle *b1=data.asList())
187  {
188  if (Bottle *b2=b1->find("in").asList())
189  {
190  in.resize(b2->size());
191  for (size_t i=0; i<in.length(); i++)
192  in[i]=b2->get(i).asFloat64();
193  }
194 
195  if (Bottle *b2=b1->find("out").asList())
196  {
197  out.resize(b2->size());
198  for (size_t i=0; i<out.length(); i++)
199  out[i]=b2->get(i).asFloat64();
200 
201  ret=true;
202  }
203  }
204  }
205 
206  return ret;
207 }
208 
209 
210 /************************************************************************/
211 bool SpringyFinger::getOutput(Value &out) const
212 {
213  Vector i,o;
214  if (!extractSensorsData(i,o))
215  return false;
216 
217  i[0]=scaler.transform(i[0]);
218  Vector pred=lssvm.predict(i).getPrediction();
219 
220  for (size_t j=0; j<pred.length(); j++)
221  pred[j]=scaler.unTransform(pred[j]);
222 
223  out=Value(outputGain*norm(o-pred));
224 
225  return true;
226 }
227 
228 
229 /************************************************************************/
230 bool SpringyFinger::calibrate(const Property &options)
231 {
232  if (options.check("reset"))
233  lssvm.reset();
234 
235  if (options.check("feed"))
236  {
237  Vector in,out;
238  if (extractSensorsData(in,out))
239  {
240  in[0]=scaler.transform(in[0]);
241  for (size_t i=0; i<out.length(); i++)
242  out[i]=scaler.transform(out[i]);
243 
244  lssvm.feedSample(in,out);
245  }
246  else
247  return false;
248  }
249 
250  if (options.check("train"))
251  {
252  lssvm.train();
253  calibrated=true;
254  }
255 
256  return true;
257 }
258 
259 
260 /************************************************************************/
261 SpringyFingersModel::SpringyFingersModel()
262 {
263  port=new iCub::perception::Port;
264  configured=false;
265 }
266 
267 
268 /************************************************************************/
269 bool SpringyFingersModel::fromProperty(const Property &options)
270 {
271  if (!options.check("name") || !options.check("type"))
272  {
273  printMessage(log::error,1,"missing mandatory options \"name\" and/or \"type\"");
274  return false;
275  }
276 
277  if (configured)
278  close();
279 
280  name=options.find("name").asString();
281  type=options.find("type").asString();
282  robot=options.check("robot",Value("icub")).asString();
283  carrier=options.check("carrier",Value("udp")).asString();
284  verbosity=options.check("verbosity",Value(0)).asInt32();
285 
286  string part_motor=string(type+"_arm");
287  string part_analog=string(type+"_hand");
288 
289  Property prop;
290  prop.put("device","remote_controlboard");
291  prop.put("remote","/"+robot+"/"+part_motor);
292  prop.put("local","/"+name+"/"+part_motor);
293  if (!driver.open(prop))
294  return false;
295 
296  port->open("/"+name+"/"+part_analog+"/analog:i");
297  string analogPortName("/"+robot+"/"+part_analog+"/analog:o");
298  if (!Network::connect(analogPortName,port->getName(),carrier))
299  {
300  printMessage(log::error,1,"unable to connect to %s",analogPortName.c_str());
301  close();
302  return false;
303  }
304 
305  IEncoders *ienc; driver.view(ienc);
306  int nAxes; ienc->getAxes(&nAxes);
307 
308  printMessage(log::info,1,"configuring interface-based sensors ...");
309  Property propGen;
310  propGen.put("name","In_0");
311  propGen.put("size",nAxes);
312 
313  Property propThumb=propGen; propThumb.put( "index",10);
314  Property propIndex=propGen; propIndex.put( "index",12);
315  Property propMiddle=propGen; propMiddle.put("index",14);
316  Property propRing=propGen; propRing.put( "index",15);
317  Property propLittle=propGen; propLittle.put("index",15);
318 
319  bool sensors_ok=true;
320  void *pEncs=static_cast<void*>(ienc);
321  sensors_ok&=sensEncs[0].configure(pEncs,propThumb);
322  sensors_ok&=sensEncs[1].configure(pEncs,propIndex);
323  sensors_ok&=sensEncs[2].configure(pEncs,propMiddle);
324  sensors_ok&=sensEncs[3].configure(pEncs,propRing);
325  sensors_ok&=sensEncs[4].configure(pEncs,propLittle);
326 
327  printMessage(log::info,1,"configuring port-based sensors ...");
328  Property thumb_mp( "(name Out_0) (index 1)" );
329  Property thumb_ip( "(name Out_1) (index 2)" );
330  Property index_mp( "(name Out_0) (index 4)" );
331  Property index_ip( "(name Out_1) (index 5)" );
332  Property middle_mp( "(name Out_0) (index 7)" );
333  Property middle_ip( "(name Out_1) (index 8)" );
334  Property ring_mp( "(name Out_0) (index 9)" );
335  Property ring_pip( "(name Out_1) (index 10)");
336  Property ring_dip( "(name Out_2) (index 11)");
337  Property little_mp( "(name Out_0) (index 12)");
338  Property little_pip("(name Out_1) (index 13)");
339  Property little_dip("(name Out_2) (index 14)");
340 
341  void *pPort=static_cast<void*>(port);
342  sensors_ok&=sensPort[0].configure(pPort,thumb_mp);
343  sensors_ok&=sensPort[1].configure(pPort,thumb_ip);
344  sensors_ok&=sensPort[2].configure(pPort,index_mp);
345  sensors_ok&=sensPort[3].configure(pPort,index_ip);
346  sensors_ok&=sensPort[4].configure(pPort,middle_mp);
347  sensors_ok&=sensPort[5].configure(pPort,middle_ip);
348  sensors_ok&=sensPort[6].configure(pPort,ring_mp);
349  sensors_ok&=sensPort[7].configure(pPort,ring_pip);
350  sensors_ok&=sensPort[8].configure(pPort,ring_dip);
351  sensors_ok&=sensPort[9].configure(pPort,little_mp);
352  sensors_ok&=sensPort[10].configure(pPort,little_pip);
353  sensors_ok&=sensPort[11].configure(pPort,little_dip);
354 
355  if (!sensors_ok)
356  {
357  printMessage(log::error,1,"some errors occured");
358  close();
359  return false;
360  }
361 
362  printMessage(log::info,1,"configuring fingers ...");
363  Property thumb(options.findGroup("thumb").toString().c_str());
364  Property index(options.findGroup("index").toString().c_str());
365  Property middle(options.findGroup("middle").toString().c_str());
366  Property ring(options.findGroup("ring").toString().c_str());
367  Property little(options.findGroup("little").toString().c_str());
368 
369  bool fingers_ok=true;
370  fingers_ok&=fingers[0].fromProperty(thumb);
371  fingers_ok&=fingers[1].fromProperty(index);
372  fingers_ok&=fingers[2].fromProperty(middle);
373  fingers_ok&=fingers[3].fromProperty(ring);
374  fingers_ok&=fingers[4].fromProperty(little);
375 
376  if (!fingers_ok)
377  {
378  printMessage(log::error,1,"some errors occured");
379  close();
380  return false;
381  }
382 
383  printMessage(log::info,1,"attaching sensors to fingers ...");
384  fingers[0].attachSensor(sensEncs[0]);
385  fingers[0].attachSensor(sensPort[0]);
386  fingers[0].attachSensor(sensPort[1]);
387 
388  fingers[1].attachSensor(sensEncs[1]);
389  fingers[1].attachSensor(sensPort[2]);
390  fingers[1].attachSensor(sensPort[3]);
391 
392  fingers[2].attachSensor(sensEncs[2]);
393  fingers[2].attachSensor(sensPort[4]);
394  fingers[2].attachSensor(sensPort[5]);
395 
396  fingers[3].attachSensor(sensEncs[3]);
397  fingers[3].attachSensor(sensPort[6]);
398  fingers[3].attachSensor(sensPort[7]);
399  fingers[3].attachSensor(sensPort[8]);
400 
401  fingers[4].attachSensor(sensEncs[4]);
402  fingers[4].attachSensor(sensPort[9]);
403  fingers[4].attachSensor(sensPort[10]);
404  fingers[4].attachSensor(sensPort[11]);
405 
406  attachNode(fingers[0]);
407  attachNode(fingers[1]);
408  attachNode(fingers[2]);
409  attachNode(fingers[3]);
410  attachNode(fingers[4]);
411 
412  printMessage(log::info,1,"configuration complete");
413  return configured=true;
414 }
415 
416 
417 /************************************************************************/
418 void SpringyFingersModel::toProperty(Property &options) const
419 {
420  options.clear();
421 
422  if (configured)
423  {
424  Property prop[5];
425  fingers[0].toProperty(prop[0]);
426  fingers[1].toProperty(prop[1]);
427  fingers[2].toProperty(prop[2]);
428  fingers[3].toProperty(prop[3]);
429  fingers[4].toProperty(prop[4]);
430 
431  string thumb="(thumb ";
432  thumb+=prop[0].toString();
433  thumb+=")";
434 
435  string index="(index ";
436  index+=prop[1].toString();
437  index+=")";
438 
439  string middle="(middle ";
440  middle+=prop[2].toString();
441  middle+=")";
442 
443  string ring="(ring ";
444  ring+=prop[3].toString();
445  ring+=")";
446 
447  string little="(little ";
448  little+=prop[4].toString();
449  little+=")";
450 
451  options.fromString(thumb+index+middle+ring+little);
452  options.put("name",name);
453  options.put("type",type);
454  options.put("robot",robot);
455  options.put("verbosity",verbosity);
456  }
457 }
458 
459 
460 /************************************************************************/
461 bool SpringyFingersModel::toStream(ostream &str) const
462 {
463  if (configured)
464  {
465  str<<"name "<<name<<endl;
466  str<<"type "<<type<<endl;
467  str<<"robot "<<robot<<endl;
468  str<<"verbosity "<<verbosity<<endl;
469 
470  str<<endl;
471  str<<"[thumb]"<<endl;
472  fingers[0].toStream(str);
473 
474  str<<endl;
475  str<<"[index]"<<endl;
476  fingers[1].toStream(str);
477 
478  str<<endl;
479  str<<"[middle]"<<endl;
480  fingers[2].toStream(str);
481 
482  str<<endl;
483  str<<"[ring]"<<endl;
484  fingers[3].toStream(str);
485 
486  str<<endl;
487  str<<"[little]"<<endl;
488  fingers[4].toStream(str);
489 
490  return true;
491  }
492  else
493  return false;
494 }
495 
496 
497 /************************************************************************/
498 bool SpringyFingersModel::calibrate(const Property &options)
499 {
500  if (configured)
501  {
502  Value fng=options.find("finger");
503  if (fng.isNull())
504  {
505  printMessage(log::error,1,"unspecified option \"finger\"");
506  return false;
507  }
508 
509  IControlMode *imod; driver.view(imod);
510  IControlLimits *ilim; driver.view(ilim);
511  IEncoders *ienc; driver.view(ienc);
512  IPositionControl *ipos; driver.view(ipos);
513 
514  int nAxes; ienc->getAxes(&nAxes);
515  Vector qmin(nAxes),qmax(nAxes),vel(nAxes),acc(nAxes);
516 
517  printMessage(log::info,1,"steering the hand to a suitable starting configuration");
518  for (int j=7; j<nAxes; j++)
519  {
520  imod->setControlMode(j,VOCAB_CM_POSITION);
521  ilim->getLimits(j,&qmin[j],&qmax[j]);
522 
523  ipos->getRefAcceleration(j,&acc[j]);
524  ipos->getRefSpeed(j,&vel[j]);
525 
526  ipos->setRefAcceleration(j,std::numeric_limits<double>::max());
527  ipos->setRefSpeed(j,60.0);
528  ipos->positionMove(j,(j==8)?qmax[j]:qmin[j]); // thumb in opposition
529  }
530 
531  printMessage(log::info,1,"proceeding with the calibration");
532  bool ok=true;
533 
534  if (fng.isString())
535  {
536  string tag=options.check("finger",Value("all")).asString();
537  if (tag=="thumb")
538  {
539  calibrateFinger(fingers[0],10,qmin[10],qmax[10]);
540  }
541  else if (tag=="index")
542  {
543  calibrateFinger(fingers[1],12,qmin[12],qmax[12]);
544  }
545  else if (tag=="middle")
546  {
547  calibrateFinger(fingers[2],14,qmin[14],qmax[14]);
548  }
549  else if (tag=="ring")
550  {
551  calibrateFinger(fingers[3],15,qmin[15],qmax[15]);
552  }
553  else if (tag=="little")
554  {
555  calibrateFinger(fingers[4],15,qmin[15],qmax[15]);
556  }
557  else if ((tag=="all") || (tag=="all_serial"))
558  {
559  calibrateFinger(fingers[0],10,qmin[10],qmax[10]);
560  calibrateFinger(fingers[1],12,qmin[12],qmax[12]);
561  calibrateFinger(fingers[2],14,qmin[14],qmax[14]);
562  calibrateFinger(fingers[3],15,qmin[15],qmax[15]);
563  calibrateFinger(fingers[4],15,qmin[15],qmax[15]);
564  }
565  else if (tag=="all_parallel")
566  {
567  Bottle b;
568  Bottle &bl=b.addList();
569  bl.addString("thumb");
570  bl.addString("index");
571  bl.addString("middle");
572  bl.addString("ring");
573  bl.addString("little");
574 
575  fng=b.get(0);
576  }
577  else
578  ok=false;
579  }
580 
581  if (fng.isList())
582  {
583  deque<CalibThread*> db;
584  set<string> singletons;
585 
586  Bottle *items=fng.asList();
587  for (int i=0; i<items->size(); i++)
588  {
589  string tag=items->get(i).asString();
590  if (tag=="thumb")
591  {
592  if (singletons.find(tag)==singletons.end())
593  {
594  CalibThread *thr=new CalibThread;
595  thr->setInfo(this,fingers[0],10,qmin[10],qmax[10]);
596  thr->start();
597  db.push_back(thr);
598  singletons.insert(tag);
599  }
600  }
601  else if (tag=="index")
602  {
603  if (singletons.find(tag)==singletons.end())
604  {
605  CalibThread *thr=new CalibThread;
606  thr->setInfo(this,fingers[1],12,qmin[12],qmax[12]);
607  thr->start();
608  db.push_back(thr);
609  singletons.insert(tag);
610  }
611  }
612  else if (tag=="middle")
613  {
614  if (singletons.find(tag)==singletons.end())
615  {
616  CalibThread *thr=new CalibThread;
617  thr->setInfo(this,fingers[2],14,qmin[14],qmax[14]);
618  thr->start();
619  db.push_back(thr);
620  singletons.insert(tag);
621  }
622  }
623  else if (tag=="ring")
624  {
625  if (singletons.find(tag)==singletons.end())
626  {
627  CalibThread *thr=new CalibThread;
628  thr->setInfo(this,fingers[3],15,qmin[15],qmax[15]);
629  thr->start();
630  db.push_back(thr);
631  singletons.insert(tag);
632  }
633  }
634  else if (tag=="little")
635  {
636  if (singletons.find(tag)==singletons.end())
637  {
638  CalibThread *thr=new CalibThread;
639  thr->setInfo(this,fingers[4],15,qmin[15],qmax[15]);
640  thr->start();
641  db.push_back(thr);
642  singletons.insert(tag);
643  }
644  }
645  }
646 
647  if (db.size()>0)
648  {
649  size_t i=0;
650  while (db.size()>0)
651  {
652  if (db[i]->isDone())
653  {
654  db[i]->stop();
655  delete db[i];
656  db.erase(db.begin()+i);
657  }
658 
659  Time::delay(0.1);
660  if (++i>=db.size())
661  i=0;
662  }
663  }
664  else
665  ok=false;
666  }
667 
668  if (!fng.isString() && !fng.isList())
669  ok=false;
670 
671  if (!ok)
672  printMessage(log::error,1,"unknown finger request %s",fng.toString().c_str());
673 
674  for (int j=7; j<nAxes; j++)
675  {
676  ipos->setRefAcceleration(j,acc[j]);
677  ipos->setRefSpeed(j,vel[j]);
678  }
679 
680  return ok;
681  }
682  else
683  return false;
684 }
685 
686 
687 /************************************************************************/
688 bool SpringyFingersModel::getOutput(Value &out) const
689 {
690  if (configured)
691  {
692  Value val[5];
693  fingers[0].getOutput(val[0]);
694  fingers[1].getOutput(val[1]);
695  fingers[2].getOutput(val[2]);
696  fingers[3].getOutput(val[3]);
697  fingers[4].getOutput(val[4]);
698 
699  Bottle bOut; Bottle &ins=bOut.addList();
700  ins.addFloat64(val[0].asFloat64());
701  ins.addFloat64(val[1].asFloat64());
702  ins.addFloat64(val[2].asFloat64());
703  ins.addFloat64(val[3].asFloat64());
704  ins.addFloat64(val[4].asFloat64());
705 
706  out=bOut.get(0);
707  return true;
708  }
709  else
710  return false;
711 }
712 
713 
714 /************************************************************************/
715 void SpringyFingersModel::calibrateFinger(SpringyFinger &finger, const int joint,
716  const double min, const double max)
717 {
718  printMessage(log::info,1,"calibrating finger %s ...",finger.getName().c_str());
719  double margin=0.1*(max-min);
720  double _min=min+margin;
721  double _max=max-margin;
722  double tol_min=5.0;
723  double tol_max=5.0;
724  double *val=&_min;
725  double *tol=&tol_min;
726  double timeout=2.0*(_max-_min)/finger.getCalibVel();
727 
728  mtx.lock();
729  IControlMode *imod; driver.view(imod);
730  IEncoders *ienc; driver.view(ienc);
731  IPositionControl *ipos; driver.view(ipos);
732  mtx.unlock();
733 
734  // workaround
735  if ((finger.getName()=="ring") || (finger.getName()=="little"))
736  {
737  _min=30.0;
738  _max=180.0;
739  tol_min=20.0;
740  tol_max=50.0;
741  }
742 
743  Property reset("(reset)");
744  Property feed("(feed)");
745  Property train("(train)");
746 
747  finger.calibrate(reset);
748 
749  mtx.lock();
750  imod->setControlMode(joint,VOCAB_CM_POSITION);
751  ipos->setRefSpeed(joint,finger.getCalibVel());
752  mtx.unlock();
753 
754  for (int i=0; i<5; i++)
755  {
756  mtx.lock();
757  ipos->positionMove(joint,*val);
758  mtx.unlock();
759 
760  bool done=false;
761  double fbOld=std::numeric_limits<double>::max();
762  double t0=Time::now();
763  while (!done)
764  {
765  Time::delay(0.01);
766 
767  mtx.lock();
768  double fb; ienc->getEncoder(joint,&fb);
769  mtx.unlock();
770 
771  if (fabs(fb-fbOld)>0.5)
772  {
773  printMessage(log::no_info,2,"feeding finger %s",finger.getName().c_str());
774  finger.calibrate(feed);
775  }
776 
777  done=(fabs(*val-fb)<*tol)||(Time::now()-t0>timeout);
778  fbOld=fb;
779  }
780 
781  if (val==&_min)
782  {
783  val=&_max;
784  tol=&tol_max;
785  }
786  else
787  {
788  val=&_min;
789  tol=&tol_min;
790  }
791  }
792 
793  printMessage(log::info,1,"training finger %s ...",finger.getName().c_str());
794  finger.calibrate(train);
795  printMessage(log::info,1,"finger %s trained!",finger.getName().c_str());
796 }
797 
798 
799 /************************************************************************/
800 bool SpringyFingersModel::isCalibrated() const
801 {
802  return (fingers[0].isCalibrated()&&
803  fingers[1].isCalibrated()&&
804  fingers[2].isCalibrated()&&
805  fingers[3].isCalibrated()&&
806  fingers[4].isCalibrated());
807 }
808 
809 
810 /************************************************************************/
811 void SpringyFingersModel::close()
812 {
813  printMessage(log::info,1,"closing ...");
814 
815  if (driver.isValid())
816  driver.close();
817 
818  if (!port->isClosed())
819  port->close();
820 
821  nodes.clear();
822 
823  configured=false;
824 }
825 
826 
827 /************************************************************************/
828 SpringyFingersModel::~SpringyFingersModel()
829 {
830  close();
831  delete port;
832 }
833 
834 
835 
iCub::perception::SpringyFinger::getCalibVel
double getCalibVel() const
Return the finger actuation velocity used while calibrating.
Definition: springyFingers.h:190
iCub::action::log::info
@ info
Definition: actionPrimitives.cpp:64
models.h
out
out
Definition: sine.m:8
strain::dsp::fsc::min
const FSC min
Definition: strain.h:49
verbosity
int verbosity
Definition: main.cpp:21
iCub::perception::SpringyFinger
Definition: springyFingers.h:84
yarp::dev
Definition: DebugInterfaces.h:52
data
@ data
Definition: ST_M1_dataType.h:64
math.h
strain::dsp::fsc::max
const FSC max
Definition: strain.h:48
iCub::action::log::no_info
@ no_info
Definition: actionPrimitives.cpp:64
iCub::perception::Node::getName
std::string getName() const
Retrieve the node name.
Definition: nodes.h:112
iCub::ctrl
Definition: adaptWinPolyEstimator.h:37
iCub::perception::Port
Definition: ports.h:34
iCub::learningmachine
Definition: DatasetRecorder.h:28
iCub::ctrl::norm
double norm(const yarp::sig::Matrix &M, int col)
state::ok
@ ok
iCub::action::log::error
@ error
Definition: actionPrimitives.cpp:64
string
string(REPLACE "-rdynamic" "" CMAKE_SHARED_LIBRARY_LINK_CXX_FLAGS "${CMAKE_SHARED_LIBRARY_LINK_CXX_FLAGS}") include_directories($
Definition: CMakeLists.txt:9
done
bool done
Definition: main.cpp:42
ports.h
acc
_3f_vect_t acc
Definition: dataTypes.h:3
springyFingers.h
iCub::skinManager::calibrate
@ calibrate
Definition: rpcSkinManager.h:13
iCub::perception::SpringyFinger::calibrate
bool calibrate(const yarp::os::Property &options)
Allow to send calibration commands to the finger.
Definition: springyFingers.cpp:230
iCub::perception
Definition: models.h:52