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>
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
59
61 lssvm.setC(1e4);
62 lssvm.getKernel()->setGamma(500.0);
63
64 double defaultCalibVel;
65 if ((name=="thumb") || (name=="index") || (name=="middle"))
66 {
68 defaultCalibVel=30.0;
69 }
70 else if ((name=="ring") || (name=="little"))
71 {
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/************************************************************************/
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/************************************************************************/
179bool 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/************************************************************************/
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/************************************************************************/
230bool 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/************************************************************************/
262{
263 configured=false;
264}
265
266
267/************************************************************************/
268bool 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/************************************************************************/
425void 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/************************************************************************/
468bool 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/************************************************************************/
505bool 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/************************************************************************/
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/************************************************************************/
722void 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/************************************************************************/
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/************************************************************************/
818void 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/************************************************************************/
839
840
841
@ data
virtual void setUpperBoundOut(double uo)
Mutator for the desired upper bound.
virtual void setUpperBoundIn(double ui)
Mutator for the expected upper bound.
virtual void setLowerBoundOut(double lo)
Mutator for the desired lower bound.
virtual void setLowerBoundIn(double li)
Mutator for the expected lower bound.
unsigned int getCoDomainSize() const
Returns the size (dimensionality) of the output domain (codomain).
unsigned int getDomainSize() const
Returns the size (dimensionality) of the input domain.
virtual bool fromString(const std::string &str)
Asks the learning machine to initialize from a string serialization.
virtual std::string toString()
Asks the learning machine to return a string serialization.
virtual std::string toString()
Asks the scaler to return a string serialization.
Definition IScaler.cpp:49
virtual double transform(double val)
Transforms a single sample value according to the state of the scaler.
Definition IScaler.cpp:30
virtual bool fromString(const std::string &str)
Asks the scaler to initialize from a string serialization.
Definition IScaler.cpp:55
virtual double unTransform(double val)
Untransforms a single sample value according to the state of the scaler.
Definition IScaler.h:116
virtual void feedSample(const yarp::sig::Vector &input, const yarp::sig::Vector &output)
Provide the learning machine with an example of the desired mapping.
void setCoDomainSize(unsigned int size)
Mutator for the codomain size.
void reset()
Forget everything and start over.
void setDomainSize(unsigned int size)
Mutator for the domain size.
virtual void setC(double C)
Mutator for the regularization parameter C.
Prediction predict(const yarp::sig::Vector &input)
Ask the learning machine to predict the output for a given input.
virtual void train()
Train the learning machine on the examples that have been supplied so far.
virtual RBFKernel * getKernel()
Accessor for the kernel.
yarp::sig::Vector getPrediction()
Accessor for the expected value of the prediction.
Definition Prediction.h:106
virtual void setGamma(double g)
virtual void printMessage(const int logtype, const int level, const char *format,...) const
Definition models.cpp:39
void attachNode(Node &node)
Attach a node object to the model.
Definition models.cpp:73
std::map< std::string, Node * > nodes
Definition models.h:67
std::string name
Definition models.h:64
std::map< std::string, EventCallback * > callbacks
Definition nodes.h:99
std::map< std::string, Node * > neighbors
Definition nodes.h:100
std::map< std::string, Sensor * > sensors
Definition nodes.h:98
void attachSensor(Sensor &sensor)
Attach a sensor object to the node.
Definition nodes.cpp:40
std::string name
Definition nodes.h:96
std::string getName() const
Retrieve the node name.
Definition nodes.h:112
bool configure(void *source, const yarp::os::Property &options)
Configure the sensor.
Definition sensors.cpp:74
bool configure(void *source, const yarp::os::Property &options)
Configure the sensor.
Definition sensors.cpp:44
An implementation of the Node class that represents the springy finger.
bool fromProperty(const yarp::os::Property &options)
Configure the finger taking its parameters from a Property object.
bool getOutput(yarp::os::Value &out) const
Retrieve the finger output.
void toProperty(yarp::os::Property &options) const
Return a Property representation of all the node parameters.
bool toStream(std::ostream &str) const
Similar to the toProperty() method but it operates on output streams (e.g.
iCub::learningmachine::LSSVMLearner lssvm
double getCalibVel() const
Return the finger actuation velocity used while calibrating.
bool getSensorsData(yarp::os::Value &data) const
Retrieve data finger from the joints used both for calibration and normal operation.
bool extractSensorsData(yarp::sig::Vector &in, yarp::sig::Vector &out) const
bool calibrate(const yarp::os::Property &options)
Allow to send calibration commands to the finger.
iCub::learningmachine::FixedRangeScaler scaler
void toProperty(yarp::os::Property &options) const
Return a Property representation of all the model parameters.
bool toStream(std::ostream &str) const
Similar to the toProperty() method but it operates on output streams (e.g.
bool isCalibrated() const
Return the internal status of the calibration.
bool calibrate(const yarp::os::Property &options)
Execute the fingers calibration.
bool fromProperty(const yarp::os::Property &options)
Configure the model taking its parameters from a Property object.
bool getOutput(yarp::os::Value &out) const
Retrieve the complete output of the model.
_3f_vect_t acc
Definition dataTypes.h:1
double norm(const yarp::sig::Matrix &M, int col)
Returns the norm of the vector given in the form: matrix(:,col).
bool done
Definition main.cpp:42
out
Definition sine.m:8