25 #include <yarp/math/Math.h>
33 using namespace yarp::os;
35 using namespace yarp::sig;
36 using namespace yarp::math;
43 bool SpringyFinger::fromProperty(
const Property &options)
45 if (!options.check(
"name"))
53 name=options.find(
"name").asString();
55 scaler.setLowerBoundIn(0.0);
56 scaler.setUpperBoundIn(255.0);
57 scaler.setLowerBoundOut(0.0);
58 scaler.setUpperBoundOut(1.0);
60 lssvm.setDomainSize(1);
62 lssvm.getKernel()->setGamma(500.0);
64 double defaultCalibVel;
65 if ((name==
"thumb") || (name==
"index") || (name==
"middle"))
67 lssvm.setCoDomainSize(2);
70 else if ((name==
"ring") || (name==
"little"))
72 lssvm.setCoDomainSize(3);
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");
82 if (options.check(
"scaler"))
84 Bottle *pB=options.find(
"scaler").asList();
85 scaler.fromString(pB->toString());
88 if (options.check(
"lssvm"))
90 Bottle *pB=options.find(
"lssvm").asList();
91 lssvm.fromString(pB->toString());
99 void SpringyFinger::toProperty(Property &options)
const
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()+
")");
112 bool SpringyFinger::toStream(ostream &str)
const
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;
126 bool SpringyFinger::getSensorsData(Value &
data)
const
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");
134 ok&=In_0!=sensors.end();
135 ok&=Out_0!=sensors.end();
136 ok&=Out_1!=sensors.end();
138 if (lssvm.getCoDomainSize()>2)
139 ok&=Out_2!=sensors.end();
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]);
149 Vector in(lssvm.getDomainSize());
150 in[0]=val_in.asFloat64();
152 Vector
out(lssvm.getCoDomainSize());
153 out[0]=val_out[0].asFloat64();
154 out[1]=val_out[1].asFloat64();
156 if (lssvm.getCoDomainSize()>2)
158 Out_2->second->getOutput(val_out[2]);
159 out[2]=val_out[2].asFloat64();
165 b.addList().read(in);
166 prop.put(
"in",b.get(0));
168 b.addList().read(
out);
169 prop.put(
"out",b.get(1));
171 b.addList().read(prop);
179 bool SpringyFinger::extractSensorsData(Vector &in, Vector &
out)
const
184 if (getSensorsData(
data))
186 if (Bottle *b1=
data.asList())
188 if (Bottle *b2=b1->find(
"in").asList())
190 in.resize(b2->size());
191 for (
size_t i=0; i<in.length(); i++)
192 in[i]=b2->get(i).asFloat64();
195 if (Bottle *b2=b1->find(
"out").asList())
197 out.resize(b2->size());
198 for (
size_t i=0; i<
out.length(); i++)
199 out[i]=b2->get(i).asFloat64();
211 bool SpringyFinger::getOutput(Value &
out)
const
214 if (!extractSensorsData(i,o))
217 i[0]=scaler.transform(i[0]);
218 Vector pred=lssvm.predict(i).getPrediction();
220 for (
size_t j=0; j<pred.length(); j++)
221 pred[j]=scaler.unTransform(pred[j]);
223 out=Value(outputGain*
norm(o-pred));
232 if (options.check(
"reset"))
235 if (options.check(
"feed"))
238 if (extractSensorsData(in,
out))
240 in[0]=scaler.transform(in[0]);
241 for (
size_t i=0; i<
out.length(); i++)
242 out[i]=scaler.transform(
out[i]);
244 lssvm.feedSample(in,
out);
250 if (options.check(
"train"))
261 SpringyFingersModel::SpringyFingersModel()
269 bool SpringyFingersModel::fromProperty(
const Property &options)
271 if (!options.check(
"name") || !options.check(
"type"))
273 printMessage(
log::error,1,
"missing mandatory options \"name\" and/or \"type\"");
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();
286 string part_motor=string(type+
"_arm");
287 string part_analog=string(type+
"_hand");
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))
296 port->open(
"/"+name+
"/"+part_analog+
"/analog:i");
297 string analogPortName(
"/"+robot+
"/"+part_analog+
"/analog:o");
298 if (!Network::connect(analogPortName,port->getName(),carrier))
300 printMessage(
log::error,1,
"unable to connect to %s",analogPortName.c_str());
305 IEncoders *ienc; driver.view(ienc);
306 int nAxes; ienc->getAxes(&nAxes);
308 printMessage(
log::info,1,
"configuring interface-based sensors ...");
310 propGen.put(
"name",
"In_0");
311 propGen.put(
"size",nAxes);
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);
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);
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)");
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);
357 printMessage(
log::error,1,
"some errors occured");
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());
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);
378 printMessage(
log::error,1,
"some errors occured");
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]);
388 fingers[1].attachSensor(sensEncs[1]);
389 fingers[1].attachSensor(sensPort[2]);
390 fingers[1].attachSensor(sensPort[3]);
392 fingers[2].attachSensor(sensEncs[2]);
393 fingers[2].attachSensor(sensPort[4]);
394 fingers[2].attachSensor(sensPort[5]);
396 fingers[3].attachSensor(sensEncs[3]);
397 fingers[3].attachSensor(sensPort[6]);
398 fingers[3].attachSensor(sensPort[7]);
399 fingers[3].attachSensor(sensPort[8]);
401 fingers[4].attachSensor(sensEncs[4]);
402 fingers[4].attachSensor(sensPort[9]);
403 fingers[4].attachSensor(sensPort[10]);
404 fingers[4].attachSensor(sensPort[11]);
406 attachNode(fingers[0]);
407 attachNode(fingers[1]);
408 attachNode(fingers[2]);
409 attachNode(fingers[3]);
410 attachNode(fingers[4]);
412 printMessage(
log::info,1,
"configuration complete");
413 return configured=
true;
418 void SpringyFingersModel::toProperty(Property &options)
const
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]);
431 string thumb=
"(thumb ";
432 thumb+=prop[0].toString();
435 string index=
"(index ";
436 index+=prop[1].toString();
439 string middle=
"(middle ";
440 middle+=prop[2].toString();
443 string ring=
"(ring ";
444 ring+=prop[3].toString();
447 string little=
"(little ";
448 little+=prop[4].toString();
451 options.fromString(thumb+index+middle+ring+little);
452 options.put(
"name",name);
453 options.put(
"type",type);
454 options.put(
"robot",robot);
461 bool SpringyFingersModel::toStream(ostream &str)
const
465 str<<
"name "<<name<<endl;
466 str<<
"type "<<type<<endl;
467 str<<
"robot "<<robot<<endl;
471 str<<
"[thumb]"<<endl;
472 fingers[0].toStream(str);
475 str<<
"[index]"<<endl;
476 fingers[1].toStream(str);
479 str<<
"[middle]"<<endl;
480 fingers[2].toStream(str);
484 fingers[3].toStream(str);
487 str<<
"[little]"<<endl;
488 fingers[4].toStream(str);
502 Value fng=options.find(
"finger");
505 printMessage(
log::error,1,
"unspecified option \"finger\"");
509 IControlMode *imod; driver.view(imod);
510 IControlLimits *ilim; driver.view(ilim);
511 IEncoders *ienc; driver.view(ienc);
512 IPositionControl *ipos; driver.view(ipos);
514 int nAxes; ienc->getAxes(&nAxes);
515 Vector qmin(nAxes),qmax(nAxes),vel(nAxes),
acc(nAxes);
517 printMessage(
log::info,1,
"steering the hand to a suitable starting configuration");
518 for (
int j=7; j<nAxes; j++)
520 imod->setControlMode(j,VOCAB_CM_POSITION);
521 ilim->getLimits(j,&qmin[j],&qmax[j]);
523 ipos->getRefAcceleration(j,&
acc[j]);
524 ipos->getRefSpeed(j,&vel[j]);
527 ipos->setRefSpeed(j,60.0);
528 ipos->positionMove(j,(j==8)?qmax[j]:qmin[j]);
531 printMessage(
log::info,1,
"proceeding with the calibration");
536 string tag=options.check(
"finger",Value(
"all")).asString();
539 calibrateFinger(fingers[0],10,qmin[10],qmax[10]);
541 else if (tag==
"index")
543 calibrateFinger(fingers[1],12,qmin[12],qmax[12]);
545 else if (tag==
"middle")
547 calibrateFinger(fingers[2],14,qmin[14],qmax[14]);
549 else if (tag==
"ring")
551 calibrateFinger(fingers[3],15,qmin[15],qmax[15]);
553 else if (tag==
"little")
555 calibrateFinger(fingers[4],15,qmin[15],qmax[15]);
557 else if ((tag==
"all") || (tag==
"all_serial"))
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]);
565 else if (tag==
"all_parallel")
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");
583 deque<CalibThread*> db;
584 set<string> singletons;
586 Bottle *items=fng.asList();
587 for (
int i=0; i<items->size(); i++)
589 string tag=items->get(i).asString();
592 if (singletons.find(tag)==singletons.end())
594 CalibThread *thr=
new CalibThread;
595 thr->setInfo(
this,fingers[0],10,qmin[10],qmax[10]);
598 singletons.insert(tag);
601 else if (tag==
"index")
603 if (singletons.find(tag)==singletons.end())
605 CalibThread *thr=
new CalibThread;
606 thr->setInfo(
this,fingers[1],12,qmin[12],qmax[12]);
609 singletons.insert(tag);
612 else if (tag==
"middle")
614 if (singletons.find(tag)==singletons.end())
616 CalibThread *thr=
new CalibThread;
617 thr->setInfo(
this,fingers[2],14,qmin[14],qmax[14]);
620 singletons.insert(tag);
623 else if (tag==
"ring")
625 if (singletons.find(tag)==singletons.end())
627 CalibThread *thr=
new CalibThread;
628 thr->setInfo(
this,fingers[3],15,qmin[15],qmax[15]);
631 singletons.insert(tag);
634 else if (tag==
"little")
636 if (singletons.find(tag)==singletons.end())
638 CalibThread *thr=
new CalibThread;
639 thr->setInfo(
this,fingers[4],15,qmin[15],qmax[15]);
642 singletons.insert(tag);
656 db.erase(db.begin()+i);
668 if (!fng.isString() && !fng.isList())
672 printMessage(
log::error,1,
"unknown finger request %s",fng.toString().c_str());
674 for (
int j=7; j<nAxes; j++)
676 ipos->setRefAcceleration(j,
acc[j]);
677 ipos->setRefSpeed(j,vel[j]);
688 bool SpringyFingersModel::getOutput(Value &
out)
const
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]);
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());
715 void SpringyFingersModel::calibrateFinger(
SpringyFinger &finger,
const int joint,
716 const double min,
const double max)
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;
725 double *tol=&tol_min;
726 double timeout=2.0*(_max-_min)/finger.
getCalibVel();
729 IControlMode *imod; driver.view(imod);
730 IEncoders *ienc; driver.view(ienc);
731 IPositionControl *ipos; driver.view(ipos);
743 Property reset(
"(reset)");
744 Property feed(
"(feed)");
745 Property train(
"(train)");
750 imod->setControlMode(joint,VOCAB_CM_POSITION);
754 for (
int i=0; i<5; i++)
757 ipos->positionMove(joint,*val);
762 double t0=Time::now();
768 double fb; ienc->getEncoder(joint,&fb);
771 if (fabs(fb-fbOld)>0.5)
777 done=(fabs(*val-fb)<*tol)||(Time::now()-t0>timeout);
793 printMessage(
log::info,1,
"training finger %s ...",finger.
getName().c_str());
800 bool SpringyFingersModel::isCalibrated()
const
802 return (fingers[0].isCalibrated()&&
803 fingers[1].isCalibrated()&&
804 fingers[2].isCalibrated()&&
805 fingers[3].isCalibrated()&&
806 fingers[4].isCalibrated());
811 void SpringyFingersModel::close()
815 if (driver.isValid())
818 if (!port->isClosed())
828 SpringyFingersModel::~SpringyFingersModel()
std::string getName() const
Retrieve the node name.
An implementation of the Node class that represents the springy finger.
double getCalibVel() const
Return the finger actuation velocity used while calibrating.
bool calibrate(const yarp::os::Property &options)
Allow to send calibration commands to the finger.
double norm(const yarp::sig::Matrix &M, int col)
Returns the norm of the vector given in the form: matrix(:,col).