iCub-main
Loading...
Searching...
No Matches
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
32using namespace std;
33using namespace yarp::os;
34using namespace yarp::dev;
35using namespace yarp::sig;
36using namespace yarp::math;
37using namespace iCub::ctrl;
38using namespace iCub::learningmachine;
39using namespace iCub::perception;
40
41
42/************************************************************************/
43bool 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/************************************************************************/
99void 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/************************************************************************/
112bool 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/************************************************************************/
126bool 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 Bottle b;
163 Bottle &d=b.addList();
164
165 Bottle &d_in=d.addList();
166 d_in.addString("in");
167 d_in.addList().read(in);
168
169 Bottle &d_out=d.addList();
170 d_out.addString("out");
171 d_out.addList().read(out);
172
173 data=b.get(0);
174
175 return true;
176}
177
178
179/************************************************************************/
180bool SpringyFinger::extractSensorsData(Vector &in, Vector &out) const
181{
182 bool ret=false;
183
184 Value data;
185 if (getSensorsData(data))
186 {
187 if (Bottle *b1=data.asList())
188 {
189 if (Bottle *b2=b1->find("in").asList())
190 {
191 in.resize(b2->size());
192 for (size_t i=0; i<in.length(); i++)
193 in[i]=b2->get(i).asFloat64();
194 }
195
196 if (Bottle *b2=b1->find("out").asList())
197 {
198 out.resize(b2->size());
199 for (size_t i=0; i<out.length(); i++)
200 out[i]=b2->get(i).asFloat64();
201
202 ret=true;
203 }
204 }
205 }
206
207 return ret;
208}
209
210
211/************************************************************************/
212bool SpringyFinger::getOutput(Value &out) const
213{
214 Vector i,o;
215 if (!extractSensorsData(i,o))
216 return false;
217
218 i[0]=scaler.transform(i[0]);
219 Vector pred=lssvm.predict(i).getPrediction();
220
221 for (size_t j=0; j<pred.length(); j++)
222 pred[j]=scaler.unTransform(pred[j]);
223
224 out=Value(outputGain*norm(o-pred));
225
226 return true;
227}
228
229
230/************************************************************************/
231bool SpringyFinger::calibrate(const Property &options)
232{
233 if (options.check("reset"))
234 lssvm.reset();
235
236 if (options.check("feed"))
237 {
238 Vector in,out;
239 if (extractSensorsData(in,out))
240 {
241 in[0]=scaler.transform(in[0]);
242 for (size_t i=0; i<out.length(); i++)
243 out[i]=scaler.transform(out[i]);
244
245 lssvm.feedSample(in,out);
246 }
247 else
248 return false;
249 }
250
251 if (options.check("train"))
252 {
253 lssvm.train();
254 calibrated=true;
255 }
256
257 return true;
258}
259
260
261/************************************************************************/
262SpringyFingersModel::SpringyFingersModel()
263{
264 configured=false;
265}
266
267
268/************************************************************************/
269bool 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 propEncs;
290 propEncs.put("device","remote_controlboard");
291 propEncs.put("remote","/"+robot+"/"+part_motor);
292 propEncs.put("local","/"+name+"/"+part_motor);
293 if (!drvEncs.open(propEncs))
294 return false;
295
296 Property propMAIS;
297 propMAIS.put("device","multipleanalogsensorsclient");
298 propMAIS.put("remote","/"+robot+"/"+part_analog+"/MAIS");
299 propMAIS.put("local","/"+name+"/"+part_analog+"/MAIS");
300 if (!drvMAIS.open(propMAIS))
301 {
302 printMessage(log::error,1,"unable to connect to %s",propMAIS.find("remote").asString().c_str());
303 close();
304 return false;
305 }
306
307 printMessage(log::info,1,"configuring joint encoders sensors ...");
308 IEncoders *ienc; drvEncs.view(ienc);
309 int nAxes; ienc->getAxes(&nAxes);
310
311 Property propGen;
312 propGen.put("name","In_0");
313 propGen.put("size",nAxes);
314
315 Property propThumb=propGen; propThumb.put( "index",10);
316 Property propIndex=propGen; propIndex.put( "index",12);
317 Property propMiddle=propGen; propMiddle.put("index",14);
318 Property propRing=propGen; propRing.put( "index",15);
319 Property propLittle=propGen; propLittle.put("index",15);
320
321 bool sensors_ok=true;
322 void *pEncs=static_cast<void*>(ienc);
323 sensors_ok&=sensEncs[0].configure(pEncs,propThumb);
324 sensors_ok&=sensEncs[1].configure(pEncs,propIndex);
325 sensors_ok&=sensEncs[2].configure(pEncs,propMiddle);
326 sensors_ok&=sensEncs[3].configure(pEncs,propRing);
327 sensors_ok&=sensEncs[4].configure(pEncs,propLittle);
328
329 printMessage(log::info,1,"configuring analog encoders sensors ...");
330 IEncoderArrays *iencarray; drvMAIS.view(iencarray);
331
332 propGen.clear();
333 propGen.put("num_arrays",(int)iencarray->getNrOfEncoderArrays());
334 propGen.put("index_array",0);
335
336 Property thumb_mp=propGen; thumb_mp.put( "name","Out_0"); thumb_mp.put("index_element","1");
337 Property thumb_ip=propGen; thumb_ip.put( "name","Out_1"); thumb_ip.put("index_element","2");
338 Property index_mp=propGen; index_mp.put( "name","Out_0"); index_mp.put("index_element","4");
339 Property index_ip=propGen; index_ip.put( "name","Out_1"); index_ip.put("index_element","5");
340 Property middle_mp=propGen; middle_mp.put( "name","Out_0"); middle_mp.put("index_element","7");
341 Property middle_ip=propGen; middle_ip.put( "name","Out_1"); middle_ip.put("index_element","8");
342 Property ring_mp=propGen; ring_mp.put( "name","Out_0"); ring_mp.put("index_element","9");
343 Property ring_pip=propGen; ring_pip.put( "name","Out_1"); ring_pip.put("index_element","10");
344 Property ring_dip=propGen; ring_dip.put( "name","Out_2"); ring_dip.put("index_element","11");
345 Property little_mp=propGen; little_mp.put( "name","Out_0"); little_mp.put("index_element","12");
346 Property little_pip=propGen; little_pip.put("name","Out_1"); little_pip.put("index_element","13");
347 Property little_dip=propGen; little_dip.put("name","Out_2"); little_dip.put("index_element","14");
348
349 void *pEncArrays=static_cast<void*>(iencarray);
350 sensors_ok&=sensEncArrays[0].configure(pEncArrays,thumb_mp);
351 sensors_ok&=sensEncArrays[1].configure(pEncArrays,thumb_ip);
352 sensors_ok&=sensEncArrays[2].configure(pEncArrays,index_mp);
353 sensors_ok&=sensEncArrays[3].configure(pEncArrays,index_ip);
354 sensors_ok&=sensEncArrays[4].configure(pEncArrays,middle_mp);
355 sensors_ok&=sensEncArrays[5].configure(pEncArrays,middle_ip);
356 sensors_ok&=sensEncArrays[6].configure(pEncArrays,ring_mp);
357 sensors_ok&=sensEncArrays[7].configure(pEncArrays,ring_pip);
358 sensors_ok&=sensEncArrays[8].configure(pEncArrays,ring_dip);
359 sensors_ok&=sensEncArrays[9].configure(pEncArrays,little_mp);
360 sensors_ok&=sensEncArrays[10].configure(pEncArrays,little_pip);
361 sensors_ok&=sensEncArrays[11].configure(pEncArrays,little_dip);
362
363 if (!sensors_ok)
364 {
365 printMessage(log::error,1,"some errors occured");
366 close();
367 return false;
368 }
369
370 printMessage(log::info,1,"configuring fingers ...");
371 Property thumb(options.findGroup("thumb").toString().c_str());
372 Property index(options.findGroup("index").toString().c_str());
373 Property middle(options.findGroup("middle").toString().c_str());
374 Property ring(options.findGroup("ring").toString().c_str());
375 Property little(options.findGroup("little").toString().c_str());
376
377 bool fingers_ok=true;
378 fingers_ok&=fingers[0].fromProperty(thumb);
379 fingers_ok&=fingers[1].fromProperty(index);
380 fingers_ok&=fingers[2].fromProperty(middle);
381 fingers_ok&=fingers[3].fromProperty(ring);
382 fingers_ok&=fingers[4].fromProperty(little);
383
384 if (!fingers_ok)
385 {
386 printMessage(log::error,1,"some errors occured");
387 close();
388 return false;
389 }
390
391 printMessage(log::info,1,"attaching sensors to fingers ...");
392 fingers[0].attachSensor(sensEncs[0]);
393 fingers[0].attachSensor(sensEncArrays[0]);
394 fingers[0].attachSensor(sensEncArrays[1]);
395
396 fingers[1].attachSensor(sensEncs[1]);
397 fingers[1].attachSensor(sensEncArrays[2]);
398 fingers[1].attachSensor(sensEncArrays[3]);
399
400 fingers[2].attachSensor(sensEncs[2]);
401 fingers[2].attachSensor(sensEncArrays[4]);
402 fingers[2].attachSensor(sensEncArrays[5]);
403
404 fingers[3].attachSensor(sensEncs[3]);
405 fingers[3].attachSensor(sensEncArrays[6]);
406 fingers[3].attachSensor(sensEncArrays[7]);
407 fingers[3].attachSensor(sensEncArrays[8]);
408
409 fingers[4].attachSensor(sensEncs[4]);
410 fingers[4].attachSensor(sensEncArrays[9]);
411 fingers[4].attachSensor(sensEncArrays[10]);
412 fingers[4].attachSensor(sensEncArrays[11]);
413
414 attachNode(fingers[0]);
415 attachNode(fingers[1]);
416 attachNode(fingers[2]);
417 attachNode(fingers[3]);
418 attachNode(fingers[4]);
419
420 printMessage(log::info,1,"configuration complete");
421 return configured=true;
422}
423
424
425/************************************************************************/
426void SpringyFingersModel::toProperty(Property &options) const
427{
428 options.clear();
429
430 if (configured)
431 {
432 Property prop[5];
433 fingers[0].toProperty(prop[0]);
434 fingers[1].toProperty(prop[1]);
435 fingers[2].toProperty(prop[2]);
436 fingers[3].toProperty(prop[3]);
437 fingers[4].toProperty(prop[4]);
438
439 string thumb="(thumb ";
440 thumb+=prop[0].toString();
441 thumb+=")";
442
443 string index="(index ";
444 index+=prop[1].toString();
445 index+=")";
446
447 string middle="(middle ";
448 middle+=prop[2].toString();
449 middle+=")";
450
451 string ring="(ring ";
452 ring+=prop[3].toString();
453 ring+=")";
454
455 string little="(little ";
456 little+=prop[4].toString();
457 little+=")";
458
459 options.fromString(thumb+index+middle+ring+little);
460 options.put("name",name);
461 options.put("type",type);
462 options.put("robot",robot);
463 options.put("verbosity",verbosity);
464 }
465}
466
467
468/************************************************************************/
469bool SpringyFingersModel::toStream(ostream &str) const
470{
471 if (configured)
472 {
473 str<<"name "<<name<<endl;
474 str<<"type "<<type<<endl;
475 str<<"robot "<<robot<<endl;
476 str<<"verbosity "<<verbosity<<endl;
477
478 str<<endl;
479 str<<"[thumb]"<<endl;
480 fingers[0].toStream(str);
481
482 str<<endl;
483 str<<"[index]"<<endl;
484 fingers[1].toStream(str);
485
486 str<<endl;
487 str<<"[middle]"<<endl;
488 fingers[2].toStream(str);
489
490 str<<endl;
491 str<<"[ring]"<<endl;
492 fingers[3].toStream(str);
493
494 str<<endl;
495 str<<"[little]"<<endl;
496 fingers[4].toStream(str);
497
498 return true;
499 }
500 else
501 return false;
502}
503
504
505/************************************************************************/
506bool SpringyFingersModel::calibrate(const Property &options)
507{
508 if (configured)
509 {
510 Value fng=options.find("finger");
511 if (fng.isNull())
512 {
513 printMessage(log::error,1,"unspecified option \"finger\"");
514 return false;
515 }
516
517 IControlMode *imod; drvEncs.view(imod);
518 IControlLimits *ilim; drvEncs.view(ilim);
519 IEncoders *ienc; drvEncs.view(ienc);
520 IPositionControl *ipos; drvEncs.view(ipos);
521
522 int nAxes; ienc->getAxes(&nAxes);
523 Vector qmin(nAxes),qmax(nAxes),vel(nAxes),acc(nAxes);
524
525 printMessage(log::info,1,"steering the hand to a suitable starting configuration");
526 for (int j=7; j<nAxes; j++)
527 {
528 imod->setControlMode(j,VOCAB_CM_POSITION);
529 ilim->getLimits(j,&qmin[j],&qmax[j]);
530
531 ipos->getRefAcceleration(j,&acc[j]);
532 ipos->getRefSpeed(j,&vel[j]);
533
534 ipos->setRefAcceleration(j,std::numeric_limits<double>::max());
535 ipos->setRefSpeed(j,60.0);
536 ipos->positionMove(j,(j==8)?qmax[j]:qmin[j]); // thumb in opposition
537 }
538
539 printMessage(log::info,1,"proceeding with the calibration");
540 bool ok=true;
541
542 if (fng.isString())
543 {
544 string tag=options.check("finger",Value("all")).asString();
545 if (tag=="thumb")
546 {
547 calibrateFinger(fingers[0],10,qmin[10],qmax[10]);
548 }
549 else if (tag=="index")
550 {
551 calibrateFinger(fingers[1],12,qmin[12],qmax[12]);
552 }
553 else if (tag=="middle")
554 {
555 calibrateFinger(fingers[2],14,qmin[14],qmax[14]);
556 }
557 else if (tag=="ring")
558 {
559 calibrateFinger(fingers[3],15,qmin[15],qmax[15]);
560 }
561 else if (tag=="little")
562 {
563 calibrateFinger(fingers[4],15,qmin[15],qmax[15]);
564 }
565 else if ((tag=="all") || (tag=="all_serial"))
566 {
567 calibrateFinger(fingers[0],10,qmin[10],qmax[10]);
568 calibrateFinger(fingers[1],12,qmin[12],qmax[12]);
569 calibrateFinger(fingers[2],14,qmin[14],qmax[14]);
570 calibrateFinger(fingers[3],15,qmin[15],qmax[15]);
571 calibrateFinger(fingers[4],15,qmin[15],qmax[15]);
572 }
573 else if (tag=="all_parallel")
574 {
575 Bottle b;
576 Bottle &bl=b.addList();
577 bl.addString("thumb");
578 bl.addString("index");
579 bl.addString("middle");
580 bl.addString("ring");
581 bl.addString("little");
582
583 fng=b.get(0);
584 }
585 else
586 ok=false;
587 }
588
589 if (fng.isList())
590 {
591 deque<CalibThread*> db;
592 set<string> singletons;
593
594 Bottle *items=fng.asList();
595 for (int i=0; i<items->size(); i++)
596 {
597 string tag=items->get(i).asString();
598 if (tag=="thumb")
599 {
600 if (singletons.find(tag)==singletons.end())
601 {
602 CalibThread *thr=new CalibThread;
603 thr->setInfo(this,fingers[0],10,qmin[10],qmax[10]);
604 thr->start();
605 db.push_back(thr);
606 singletons.insert(tag);
607 }
608 }
609 else if (tag=="index")
610 {
611 if (singletons.find(tag)==singletons.end())
612 {
613 CalibThread *thr=new CalibThread;
614 thr->setInfo(this,fingers[1],12,qmin[12],qmax[12]);
615 thr->start();
616 db.push_back(thr);
617 singletons.insert(tag);
618 }
619 }
620 else if (tag=="middle")
621 {
622 if (singletons.find(tag)==singletons.end())
623 {
624 CalibThread *thr=new CalibThread;
625 thr->setInfo(this,fingers[2],14,qmin[14],qmax[14]);
626 thr->start();
627 db.push_back(thr);
628 singletons.insert(tag);
629 }
630 }
631 else if (tag=="ring")
632 {
633 if (singletons.find(tag)==singletons.end())
634 {
635 CalibThread *thr=new CalibThread;
636 thr->setInfo(this,fingers[3],15,qmin[15],qmax[15]);
637 thr->start();
638 db.push_back(thr);
639 singletons.insert(tag);
640 }
641 }
642 else if (tag=="little")
643 {
644 if (singletons.find(tag)==singletons.end())
645 {
646 CalibThread *thr=new CalibThread;
647 thr->setInfo(this,fingers[4],15,qmin[15],qmax[15]);
648 thr->start();
649 db.push_back(thr);
650 singletons.insert(tag);
651 }
652 }
653 }
654
655 if (db.size()>0)
656 {
657 size_t i=0;
658 while (db.size()>0)
659 {
660 if (db[i]->isDone())
661 {
662 db[i]->stop();
663 delete db[i];
664 db.erase(db.begin()+i);
665 }
666
667 Time::delay(0.1);
668 if (++i>=db.size())
669 i=0;
670 }
671 }
672 else
673 ok=false;
674 }
675
676 if (!fng.isString() && !fng.isList())
677 ok=false;
678
679 if (!ok)
680 printMessage(log::error,1,"unknown finger request %s",fng.toString().c_str());
681
682 for (int j=7; j<nAxes; j++)
683 {
684 ipos->setRefAcceleration(j,acc[j]);
685 ipos->setRefSpeed(j,vel[j]);
686 }
687
688 return ok;
689 }
690 else
691 return false;
692}
693
694
695/************************************************************************/
696bool SpringyFingersModel::getOutput(Value &out) const
697{
698 if (configured)
699 {
700 Value val[5];
701 fingers[0].getOutput(val[0]);
702 fingers[1].getOutput(val[1]);
703 fingers[2].getOutput(val[2]);
704 fingers[3].getOutput(val[3]);
705 fingers[4].getOutput(val[4]);
706
707 Bottle bOut; Bottle &ins=bOut.addList();
708 ins.addFloat64(val[0].asFloat64());
709 ins.addFloat64(val[1].asFloat64());
710 ins.addFloat64(val[2].asFloat64());
711 ins.addFloat64(val[3].asFloat64());
712 ins.addFloat64(val[4].asFloat64());
713
714 out=bOut.get(0);
715 return true;
716 }
717 else
718 return false;
719}
720
721
722/************************************************************************/
723void SpringyFingersModel::calibrateFinger(SpringyFinger &finger, const int joint,
724 const double min, const double max)
725{
726 printMessage(log::info,1,"calibrating finger %s ...",finger.getName().c_str());
727 double margin=0.1*(max-min);
728 double _min=min+margin;
729 double _max=max-margin;
730 double tol_min=5.0;
731 double tol_max=5.0;
732 double *val=&_min;
733 double *tol=&tol_min;
734 double timeout=2.0*(_max-_min)/finger.getCalibVel();
735
736 mtx.lock();
737 IControlMode *imod; drvEncs.view(imod);
738 IEncoders *ienc; drvEncs.view(ienc);
739 IPositionControl *ipos; drvEncs.view(ipos);
740 mtx.unlock();
741
742 // workaround
743 if ((finger.getName()=="ring") || (finger.getName()=="little"))
744 {
745 _min=30.0;
746 _max=180.0;
747 tol_min=20.0;
748 tol_max=50.0;
749 }
750
751 Property reset("(reset)");
752 Property feed("(feed)");
753 Property train("(train)");
754
755 finger.calibrate(reset);
756
757 mtx.lock();
758 imod->setControlMode(joint,VOCAB_CM_POSITION);
759 ipos->setRefSpeed(joint,finger.getCalibVel());
760 mtx.unlock();
761
762 for (int i=0; i<5; i++)
763 {
764 mtx.lock();
765 ipos->positionMove(joint,*val);
766 mtx.unlock();
767
768 bool done=false;
769 double fbOld=std::numeric_limits<double>::max();
770 double t0=Time::now();
771 while (!done)
772 {
773 Time::delay(0.01);
774
775 mtx.lock();
776 double fb; ienc->getEncoder(joint,&fb);
777 mtx.unlock();
778
779 if (fabs(fb-fbOld)>0.5)
780 {
781 printMessage(log::no_info,2,"feeding finger %s",finger.getName().c_str());
782 finger.calibrate(feed);
783 }
784
785 done=(fabs(*val-fb)<*tol)||(Time::now()-t0>timeout);
786 fbOld=fb;
787 }
788
789 if (val==&_min)
790 {
791 val=&_max;
792 tol=&tol_max;
793 }
794 else
795 {
796 val=&_min;
797 tol=&tol_min;
798 }
799 }
800
801 printMessage(log::info,1,"training finger %s ...",finger.getName().c_str());
802 finger.calibrate(train);
803 printMessage(log::info,1,"finger %s trained!",finger.getName().c_str());
804}
805
806
807/************************************************************************/
808bool SpringyFingersModel::isCalibrated() const
809{
810 return (fingers[0].isCalibrated()&&
811 fingers[1].isCalibrated()&&
812 fingers[2].isCalibrated()&&
813 fingers[3].isCalibrated()&&
814 fingers[4].isCalibrated());
815}
816
817
818/************************************************************************/
819void SpringyFingersModel::close()
820{
821 printMessage(log::info,1,"closing ...");
822
823 if (drvEncs.isValid())
824 drvEncs.close();
825
826 if (drvMAIS.isValid())
827 drvMAIS.close();
828
829 nodes.clear();
830
831 configured=false;
832}
833
834
835/************************************************************************/
840
841
842
data
@ data
Definition ST_M1_dataType.h:64
iCub::perception::Model::printMessage
virtual void printMessage(const int logtype, const int level, const char *format,...) const
Definition models.cpp:39
iCub::perception::Model::attachNode
void attachNode(Node &node)
Attach a node object to the model.
Definition models.cpp:73
iCub::perception::Model::nodes
std::map< std::string, Node * > nodes
Definition models.h:67
iCub::perception::Model::name
std::string name
Definition models.h:64
iCub::perception::Node::callbacks
std::map< std::string, EventCallback * > callbacks
Definition nodes.h:99
iCub::perception::Node::neighbors
std::map< std::string, Node * > neighbors
Definition nodes.h:100
iCub::perception::Node::sensors
std::map< std::string, Sensor * > sensors
Definition nodes.h:98
iCub::perception::Node::name
std::string name
Definition nodes.h:96
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::fromProperty
bool fromProperty(const yarp::os::Property &options)
Configure the finger taking its parameters from a Property object.
Definition springyFingers.cpp:43
iCub::perception::SpringyFinger::getOutput
bool getOutput(yarp::os::Value &out) const
Retrieve the finger output.
Definition springyFingers.cpp:212
iCub::perception::SpringyFinger::toProperty
void toProperty(yarp::os::Property &options) const
Return a Property representation of all the node parameters.
Definition springyFingers.cpp:99
iCub::perception::SpringyFinger::toStream
bool toStream(std::ostream &str) const
Similar to the toProperty() method but it operates on output streams (e.g.
Definition springyFingers.cpp:112
iCub::perception::SpringyFinger::lssvm
iCub::learningmachine::LSSVMLearner lssvm
Definition springyFingers.h:88
iCub::perception::SpringyFinger::getCalibVel
double getCalibVel() const
Return the finger actuation velocity used while calibrating.
Definition springyFingers.h:190
iCub::perception::SpringyFinger::getSensorsData
bool getSensorsData(yarp::os::Value &data) const
Retrieve data finger from the joints used both for calibration and normal operation.
Definition springyFingers.cpp:126
iCub::perception::SpringyFinger::extractSensorsData
bool extractSensorsData(yarp::sig::Vector &in, yarp::sig::Vector &out) const
Definition springyFingers.cpp:180
iCub::perception::SpringyFinger::calibrate
bool calibrate(const yarp::os::Property &options)
Allow to send calibration commands to the finger.
Definition springyFingers.cpp:231
iCub::perception::SpringyFinger::scaler
iCub::learningmachine::FixedRangeScaler scaler
Definition springyFingers.h:87
iCub::perception::SpringyFingersModel::toProperty
void toProperty(yarp::os::Property &options) const
Return a Property representation of all the model parameters.
Definition springyFingers.cpp:426
iCub::perception::SpringyFingersModel::toStream
bool toStream(std::ostream &str) const
Similar to the toProperty() method but it operates on output streams (e.g.
Definition springyFingers.cpp:469
iCub::perception::SpringyFingersModel::isCalibrated
bool isCalibrated() const
Return the internal status of the calibration.
Definition springyFingers.cpp:808
iCub::perception::SpringyFingersModel::calibrate
bool calibrate(const yarp::os::Property &options)
Execute the fingers calibration.
Definition springyFingers.cpp:506
iCub::perception::SpringyFingersModel::fromProperty
bool fromProperty(const yarp::os::Property &options)
Configure the model taking its parameters from a Property object.
Definition springyFingers.cpp:269
iCub::perception::SpringyFingersModel::getOutput
bool getOutput(yarp::os::Value &out) const
Retrieve the complete output of the model.
Definition springyFingers.cpp:696
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).
done
bool done
Definition main.cpp:42
out
out
Definition sine.m:8
Generated on for iCub-main by doxygen 1.15.0