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>
28 #include <iCub/perception/private/ports.h>
29 #include <iCub/perception/private/models.h>
30 #include <iCub/perception/springyFingers.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  configured=false;
264 }
265 
266 
267 /************************************************************************/
268 bool SpringyFingersModel::fromProperty(const Property &options)
269 {
270  if (!options.check("name") || !options.check("type"))
271  {
272  printMessage(log::error,1,"missing mandatory options \"name\" and/or \"type\"");
273  return false;
274  }
275 
276  if (configured)
277  close();
278 
279  name=options.find("name").asString();
280  type=options.find("type").asString();
281  robot=options.check("robot",Value("icub")).asString();
282  carrier=options.check("carrier",Value("udp")).asString();
283  verbosity=options.check("verbosity",Value(0)).asInt32();
284 
285  string part_motor=string(type+"_arm");
286  string part_analog=string(type+"_hand");
287 
288  Property propEncs;
289  propEncs.put("device","remote_controlboard");
290  propEncs.put("remote","/"+robot+"/"+part_motor);
291  propEncs.put("local","/"+name+"/"+part_motor);
292  if (!drvEncs.open(propEncs))
293  return false;
294 
295  Property propMAIS;
296  propMAIS.put("device","multipleanalogsensorsclient");
297  propMAIS.put("remote","/"+robot+"/"+part_analog+"/MAIS");
298  propMAIS.put("local","/"+name+"/"+part_analog+"/MAIS");
299  if (!drvMAIS.open(propMAIS))
300  {
301  printMessage(log::error,1,"unable to connect to %s",propMAIS.find("remote").asString().c_str());
302  close();
303  return false;
304  }
305 
306  printMessage(log::info,1,"configuring joint encoders sensors ...");
307  IEncoders *ienc; drvEncs.view(ienc);
308  int nAxes; ienc->getAxes(&nAxes);
309 
310  Property propGen;
311  propGen.put("name","In_0");
312  propGen.put("size",nAxes);
313 
314  Property propThumb=propGen; propThumb.put( "index",10);
315  Property propIndex=propGen; propIndex.put( "index",12);
316  Property propMiddle=propGen; propMiddle.put("index",14);
317  Property propRing=propGen; propRing.put( "index",15);
318  Property propLittle=propGen; propLittle.put("index",15);
319 
320  bool sensors_ok=true;
321  void *pEncs=static_cast<void*>(ienc);
322  sensors_ok&=sensEncs[0].configure(pEncs,propThumb);
323  sensors_ok&=sensEncs[1].configure(pEncs,propIndex);
324  sensors_ok&=sensEncs[2].configure(pEncs,propMiddle);
325  sensors_ok&=sensEncs[3].configure(pEncs,propRing);
326  sensors_ok&=sensEncs[4].configure(pEncs,propLittle);
327 
328  printMessage(log::info,1,"configuring analog encoders sensors ...");
329  IEncoderArrays *iencarray; drvMAIS.view(iencarray);
330 
331  propGen.clear();
332  propGen.put("num_arrays",(int)iencarray->getNrOfEncoderArrays());
333  propGen.put("index_array",0);
334 
335  Property thumb_mp=propGen; thumb_mp.put( "name","Out_0"); thumb_mp.put("index_element","1");
336  Property thumb_ip=propGen; thumb_ip.put( "name","Out_1"); thumb_ip.put("index_element","2");
337  Property index_mp=propGen; index_mp.put( "name","Out_0"); index_mp.put("index_element","4");
338  Property index_ip=propGen; index_ip.put( "name","Out_1"); index_ip.put("index_element","5");
339  Property middle_mp=propGen; middle_mp.put( "name","Out_0"); middle_mp.put("index_element","7");
340  Property middle_ip=propGen; middle_ip.put( "name","Out_1"); middle_ip.put("index_element","8");
341  Property ring_mp=propGen; ring_mp.put( "name","Out_0"); ring_mp.put("index_element","9");
342  Property ring_pip=propGen; ring_pip.put( "name","Out_1"); ring_pip.put("index_element","10");
343  Property ring_dip=propGen; ring_dip.put( "name","Out_2"); ring_dip.put("index_element","11");
344  Property little_mp=propGen; little_mp.put( "name","Out_0"); little_mp.put("index_element","12");
345  Property little_pip=propGen; little_pip.put("name","Out_1"); little_pip.put("index_element","13");
346  Property little_dip=propGen; little_dip.put("name","Out_2"); little_dip.put("index_element","14");
347 
348  void *pEncArrays=static_cast<void*>(iencarray);
349  sensors_ok&=sensEncArrays[0].configure(pEncArrays,thumb_mp);
350  sensors_ok&=sensEncArrays[1].configure(pEncArrays,thumb_ip);
351  sensors_ok&=sensEncArrays[2].configure(pEncArrays,index_mp);
352  sensors_ok&=sensEncArrays[3].configure(pEncArrays,index_ip);
353  sensors_ok&=sensEncArrays[4].configure(pEncArrays,middle_mp);
354  sensors_ok&=sensEncArrays[5].configure(pEncArrays,middle_ip);
355  sensors_ok&=sensEncArrays[6].configure(pEncArrays,ring_mp);
356  sensors_ok&=sensEncArrays[7].configure(pEncArrays,ring_pip);
357  sensors_ok&=sensEncArrays[8].configure(pEncArrays,ring_dip);
358  sensors_ok&=sensEncArrays[9].configure(pEncArrays,little_mp);
359  sensors_ok&=sensEncArrays[10].configure(pEncArrays,little_pip);
360  sensors_ok&=sensEncArrays[11].configure(pEncArrays,little_dip);
361 
362  if (!sensors_ok)
363  {
364  printMessage(log::error,1,"some errors occured");
365  close();
366  return false;
367  }
368 
369  printMessage(log::info,1,"configuring fingers ...");
370  Property thumb(options.findGroup("thumb").toString().c_str());
371  Property index(options.findGroup("index").toString().c_str());
372  Property middle(options.findGroup("middle").toString().c_str());
373  Property ring(options.findGroup("ring").toString().c_str());
374  Property little(options.findGroup("little").toString().c_str());
375 
376  bool fingers_ok=true;
377  fingers_ok&=fingers[0].fromProperty(thumb);
378  fingers_ok&=fingers[1].fromProperty(index);
379  fingers_ok&=fingers[2].fromProperty(middle);
380  fingers_ok&=fingers[3].fromProperty(ring);
381  fingers_ok&=fingers[4].fromProperty(little);
382 
383  if (!fingers_ok)
384  {
385  printMessage(log::error,1,"some errors occured");
386  close();
387  return false;
388  }
389 
390  printMessage(log::info,1,"attaching sensors to fingers ...");
391  fingers[0].attachSensor(sensEncs[0]);
392  fingers[0].attachSensor(sensEncArrays[0]);
393  fingers[0].attachSensor(sensEncArrays[1]);
394 
395  fingers[1].attachSensor(sensEncs[1]);
396  fingers[1].attachSensor(sensEncArrays[2]);
397  fingers[1].attachSensor(sensEncArrays[3]);
398 
399  fingers[2].attachSensor(sensEncs[2]);
400  fingers[2].attachSensor(sensEncArrays[4]);
401  fingers[2].attachSensor(sensEncArrays[5]);
402 
403  fingers[3].attachSensor(sensEncs[3]);
404  fingers[3].attachSensor(sensEncArrays[6]);
405  fingers[3].attachSensor(sensEncArrays[7]);
406  fingers[3].attachSensor(sensEncArrays[8]);
407 
408  fingers[4].attachSensor(sensEncs[4]);
409  fingers[4].attachSensor(sensEncArrays[9]);
410  fingers[4].attachSensor(sensEncArrays[10]);
411  fingers[4].attachSensor(sensEncArrays[11]);
412 
413  attachNode(fingers[0]);
414  attachNode(fingers[1]);
415  attachNode(fingers[2]);
416  attachNode(fingers[3]);
417  attachNode(fingers[4]);
418 
419  printMessage(log::info,1,"configuration complete");
420  return configured=true;
421 }
422 
423 
424 /************************************************************************/
425 void SpringyFingersModel::toProperty(Property &options) const
426 {
427  options.clear();
428 
429  if (configured)
430  {
431  Property prop[5];
432  fingers[0].toProperty(prop[0]);
433  fingers[1].toProperty(prop[1]);
434  fingers[2].toProperty(prop[2]);
435  fingers[3].toProperty(prop[3]);
436  fingers[4].toProperty(prop[4]);
437 
438  string thumb="(thumb ";
439  thumb+=prop[0].toString();
440  thumb+=")";
441 
442  string index="(index ";
443  index+=prop[1].toString();
444  index+=")";
445 
446  string middle="(middle ";
447  middle+=prop[2].toString();
448  middle+=")";
449 
450  string ring="(ring ";
451  ring+=prop[3].toString();
452  ring+=")";
453 
454  string little="(little ";
455  little+=prop[4].toString();
456  little+=")";
457 
458  options.fromString(thumb+index+middle+ring+little);
459  options.put("name",name);
460  options.put("type",type);
461  options.put("robot",robot);
462  options.put("verbosity",verbosity);
463  }
464 }
465 
466 
467 /************************************************************************/
468 bool SpringyFingersModel::toStream(ostream &str) const
469 {
470  if (configured)
471  {
472  str<<"name "<<name<<endl;
473  str<<"type "<<type<<endl;
474  str<<"robot "<<robot<<endl;
475  str<<"verbosity "<<verbosity<<endl;
476 
477  str<<endl;
478  str<<"[thumb]"<<endl;
479  fingers[0].toStream(str);
480 
481  str<<endl;
482  str<<"[index]"<<endl;
483  fingers[1].toStream(str);
484 
485  str<<endl;
486  str<<"[middle]"<<endl;
487  fingers[2].toStream(str);
488 
489  str<<endl;
490  str<<"[ring]"<<endl;
491  fingers[3].toStream(str);
492 
493  str<<endl;
494  str<<"[little]"<<endl;
495  fingers[4].toStream(str);
496 
497  return true;
498  }
499  else
500  return false;
501 }
502 
503 
504 /************************************************************************/
505 bool SpringyFingersModel::calibrate(const Property &options)
506 {
507  if (configured)
508  {
509  Value fng=options.find("finger");
510  if (fng.isNull())
511  {
512  printMessage(log::error,1,"unspecified option \"finger\"");
513  return false;
514  }
515 
516  IControlMode *imod; drvEncs.view(imod);
517  IControlLimits *ilim; drvEncs.view(ilim);
518  IEncoders *ienc; drvEncs.view(ienc);
519  IPositionControl *ipos; drvEncs.view(ipos);
520 
521  int nAxes; ienc->getAxes(&nAxes);
522  Vector qmin(nAxes),qmax(nAxes),vel(nAxes),acc(nAxes);
523 
524  printMessage(log::info,1,"steering the hand to a suitable starting configuration");
525  for (int j=7; j<nAxes; j++)
526  {
527  imod->setControlMode(j,VOCAB_CM_POSITION);
528  ilim->getLimits(j,&qmin[j],&qmax[j]);
529 
530  ipos->getRefAcceleration(j,&acc[j]);
531  ipos->getRefSpeed(j,&vel[j]);
532 
533  ipos->setRefAcceleration(j,std::numeric_limits<double>::max());
534  ipos->setRefSpeed(j,60.0);
535  ipos->positionMove(j,(j==8)?qmax[j]:qmin[j]); // thumb in opposition
536  }
537 
538  printMessage(log::info,1,"proceeding with the calibration");
539  bool ok=true;
540 
541  if (fng.isString())
542  {
543  string tag=options.check("finger",Value("all")).asString();
544  if (tag=="thumb")
545  {
546  calibrateFinger(fingers[0],10,qmin[10],qmax[10]);
547  }
548  else if (tag=="index")
549  {
550  calibrateFinger(fingers[1],12,qmin[12],qmax[12]);
551  }
552  else if (tag=="middle")
553  {
554  calibrateFinger(fingers[2],14,qmin[14],qmax[14]);
555  }
556  else if (tag=="ring")
557  {
558  calibrateFinger(fingers[3],15,qmin[15],qmax[15]);
559  }
560  else if (tag=="little")
561  {
562  calibrateFinger(fingers[4],15,qmin[15],qmax[15]);
563  }
564  else if ((tag=="all") || (tag=="all_serial"))
565  {
566  calibrateFinger(fingers[0],10,qmin[10],qmax[10]);
567  calibrateFinger(fingers[1],12,qmin[12],qmax[12]);
568  calibrateFinger(fingers[2],14,qmin[14],qmax[14]);
569  calibrateFinger(fingers[3],15,qmin[15],qmax[15]);
570  calibrateFinger(fingers[4],15,qmin[15],qmax[15]);
571  }
572  else if (tag=="all_parallel")
573  {
574  Bottle b;
575  Bottle &bl=b.addList();
576  bl.addString("thumb");
577  bl.addString("index");
578  bl.addString("middle");
579  bl.addString("ring");
580  bl.addString("little");
581 
582  fng=b.get(0);
583  }
584  else
585  ok=false;
586  }
587 
588  if (fng.isList())
589  {
590  deque<CalibThread*> db;
591  set<string> singletons;
592 
593  Bottle *items=fng.asList();
594  for (int i=0; i<items->size(); i++)
595  {
596  string tag=items->get(i).asString();
597  if (tag=="thumb")
598  {
599  if (singletons.find(tag)==singletons.end())
600  {
601  CalibThread *thr=new CalibThread;
602  thr->setInfo(this,fingers[0],10,qmin[10],qmax[10]);
603  thr->start();
604  db.push_back(thr);
605  singletons.insert(tag);
606  }
607  }
608  else if (tag=="index")
609  {
610  if (singletons.find(tag)==singletons.end())
611  {
612  CalibThread *thr=new CalibThread;
613  thr->setInfo(this,fingers[1],12,qmin[12],qmax[12]);
614  thr->start();
615  db.push_back(thr);
616  singletons.insert(tag);
617  }
618  }
619  else if (tag=="middle")
620  {
621  if (singletons.find(tag)==singletons.end())
622  {
623  CalibThread *thr=new CalibThread;
624  thr->setInfo(this,fingers[2],14,qmin[14],qmax[14]);
625  thr->start();
626  db.push_back(thr);
627  singletons.insert(tag);
628  }
629  }
630  else if (tag=="ring")
631  {
632  if (singletons.find(tag)==singletons.end())
633  {
634  CalibThread *thr=new CalibThread;
635  thr->setInfo(this,fingers[3],15,qmin[15],qmax[15]);
636  thr->start();
637  db.push_back(thr);
638  singletons.insert(tag);
639  }
640  }
641  else if (tag=="little")
642  {
643  if (singletons.find(tag)==singletons.end())
644  {
645  CalibThread *thr=new CalibThread;
646  thr->setInfo(this,fingers[4],15,qmin[15],qmax[15]);
647  thr->start();
648  db.push_back(thr);
649  singletons.insert(tag);
650  }
651  }
652  }
653 
654  if (db.size()>0)
655  {
656  size_t i=0;
657  while (db.size()>0)
658  {
659  if (db[i]->isDone())
660  {
661  db[i]->stop();
662  delete db[i];
663  db.erase(db.begin()+i);
664  }
665 
666  Time::delay(0.1);
667  if (++i>=db.size())
668  i=0;
669  }
670  }
671  else
672  ok=false;
673  }
674 
675  if (!fng.isString() && !fng.isList())
676  ok=false;
677 
678  if (!ok)
679  printMessage(log::error,1,"unknown finger request %s",fng.toString().c_str());
680 
681  for (int j=7; j<nAxes; j++)
682  {
683  ipos->setRefAcceleration(j,acc[j]);
684  ipos->setRefSpeed(j,vel[j]);
685  }
686 
687  return ok;
688  }
689  else
690  return false;
691 }
692 
693 
694 /************************************************************************/
695 bool SpringyFingersModel::getOutput(Value &out) const
696 {
697  if (configured)
698  {
699  Value val[5];
700  fingers[0].getOutput(val[0]);
701  fingers[1].getOutput(val[1]);
702  fingers[2].getOutput(val[2]);
703  fingers[3].getOutput(val[3]);
704  fingers[4].getOutput(val[4]);
705 
706  Bottle bOut; Bottle &ins=bOut.addList();
707  ins.addFloat64(val[0].asFloat64());
708  ins.addFloat64(val[1].asFloat64());
709  ins.addFloat64(val[2].asFloat64());
710  ins.addFloat64(val[3].asFloat64());
711  ins.addFloat64(val[4].asFloat64());
712 
713  out=bOut.get(0);
714  return true;
715  }
716  else
717  return false;
718 }
719 
720 
721 /************************************************************************/
722 void SpringyFingersModel::calibrateFinger(SpringyFinger &finger, const int joint,
723  const double min, const double max)
724 {
725  printMessage(log::info,1,"calibrating finger %s ...",finger.getName().c_str());
726  double margin=0.1*(max-min);
727  double _min=min+margin;
728  double _max=max-margin;
729  double tol_min=5.0;
730  double tol_max=5.0;
731  double *val=&_min;
732  double *tol=&tol_min;
733  double timeout=2.0*(_max-_min)/finger.getCalibVel();
734 
735  mtx.lock();
736  IControlMode *imod; drvEncs.view(imod);
737  IEncoders *ienc; drvEncs.view(ienc);
738  IPositionControl *ipos; drvEncs.view(ipos);
739  mtx.unlock();
740 
741  // workaround
742  if ((finger.getName()=="ring") || (finger.getName()=="little"))
743  {
744  _min=30.0;
745  _max=180.0;
746  tol_min=20.0;
747  tol_max=50.0;
748  }
749 
750  Property reset("(reset)");
751  Property feed("(feed)");
752  Property train("(train)");
753 
754  finger.calibrate(reset);
755 
756  mtx.lock();
757  imod->setControlMode(joint,VOCAB_CM_POSITION);
758  ipos->setRefSpeed(joint,finger.getCalibVel());
759  mtx.unlock();
760 
761  for (int i=0; i<5; i++)
762  {
763  mtx.lock();
764  ipos->positionMove(joint,*val);
765  mtx.unlock();
766 
767  bool done=false;
768  double fbOld=std::numeric_limits<double>::max();
769  double t0=Time::now();
770  while (!done)
771  {
772  Time::delay(0.01);
773 
774  mtx.lock();
775  double fb; ienc->getEncoder(joint,&fb);
776  mtx.unlock();
777 
778  if (fabs(fb-fbOld)>0.5)
779  {
780  printMessage(log::no_info,2,"feeding finger %s",finger.getName().c_str());
781  finger.calibrate(feed);
782  }
783 
784  done=(fabs(*val-fb)<*tol)||(Time::now()-t0>timeout);
785  fbOld=fb;
786  }
787 
788  if (val==&_min)
789  {
790  val=&_max;
791  tol=&tol_max;
792  }
793  else
794  {
795  val=&_min;
796  tol=&tol_min;
797  }
798  }
799 
800  printMessage(log::info,1,"training finger %s ...",finger.getName().c_str());
801  finger.calibrate(train);
802  printMessage(log::info,1,"finger %s trained!",finger.getName().c_str());
803 }
804 
805 
806 /************************************************************************/
807 bool SpringyFingersModel::isCalibrated() const
808 {
809  return (fingers[0].isCalibrated()&&
810  fingers[1].isCalibrated()&&
811  fingers[2].isCalibrated()&&
812  fingers[3].isCalibrated()&&
813  fingers[4].isCalibrated());
814 }
815 
816 
817 /************************************************************************/
818 void SpringyFingersModel::close()
819 {
820  printMessage(log::info,1,"closing ...");
821 
822  if (drvEncs.isValid())
823  drvEncs.close();
824 
825  if (drvMAIS.isValid())
826  drvMAIS.close();
827 
828  nodes.clear();
829 
830  configured=false;
831 }
832 
833 
834 /************************************************************************/
835 SpringyFingersModel::~SpringyFingersModel()
836 {
837  close();
838 }
839 
840 
841 
data
@ data
Definition: ST_M1_dataType.h:64
iCub::perception::Node::getName
std::string getName() const
Retrieve the node name.
Definition: nodes.h:112
iCub::perception::SpringyFinger
An implementation of the Node class that represents the springy finger.
Definition: springyFingers.h:85
iCub::perception::SpringyFinger::getCalibVel
double getCalibVel() const
Return the finger actuation velocity used while calibrating.
Definition: springyFingers.h:190
iCub::perception::SpringyFinger::calibrate
bool calibrate(const yarp::os::Property &options)
Allow to send calibration commands to the finger.
Definition: springyFingers.cpp:230
acc
_3f_vect_t acc
Definition: dataTypes.h:1
iCub::ctrl::norm
double norm(const yarp::sig::Matrix &M, int col)
Returns the norm of the vector given in the form: matrix(:,col).
verbosity
int verbosity
Definition: main.cpp:17
done
bool done
Definition: main.cpp:42
strain::dsp::fsc::max
const FSC max
Definition: strain.h:48
strain::dsp::fsc::min
const FSC min
Definition: strain.h:49
out
out
Definition: sine.m:8
Generated on Wed Nov 13 2024 21:11:41 for iCub-main by doxygen 1.9.1