28 #include <yarp/os/all.h>
29 #include <yarp/dev/all.h>
30 #include <yarp/sig/Vector.h>
31 #include <yarp/sig/Matrix.h>
32 #include <yarp/math/Math.h>
34 #include <iCub/ctrl/outliersDetection.h>
35 #include <iCub/optimization/calibReference.h>
37 #include "src/iolReachingCalibration_IDL.h"
39 #define ACK yarp::os::createVocab32('a','c','k')
42 using namespace yarp::os;
43 using namespace yarp::dev;
44 using namespace yarp::sig;
45 using namespace yarp::math;
46 using namespace iCub::ctrl;
47 using namespace iCub::optimization;
51 class Calibrator :
public RFModule,
58 CalibReferenceWithMatchedPoints calibrator;
59 TableEntry() : H(eye(4,4)), calibrated(false) { }
61 map<string,TableEntry> table;
63 Vector reachAboveOrientationLeft;
64 Vector reachAboveOrientationRight;
75 PolyDriver drvCartLeft;
76 PolyDriver drvCartRight;
83 bool attach(RpcServer &source)
85 return this->yarp().attachAsServer(source);
89 Vector removeOutliers(
const deque<Vector> &points)
96 for (
size_t i=0; i<points.size(); i++)
101 Vector dist(points.size());
102 for (
size_t i=0; i<points.size(); i++)
103 dist[i]=norm(points[i]-avg);
106 ModifiedThompsonTau detector;
107 set<size_t> outliers=detector.detect(dist,Property(
"(recursive)"));
111 for (
size_t i=0; i<points.size(); i++)
113 bool isInlier=(outliers.find(i)==outliers.end());
114 yInfo()<<
"point ("<<points[i].toString(3,3)<<
") at "
115 <<dist[i]<<
" [m] from ("<<avg.toString(3,3)<<
") is "
116 <<(isInlier?
"inlier":
"outlier");
135 bool getHandOrientation(ResourceFinder &rf,
const string &hand,
139 Bottle &bGroup=rf.findGroup(hand);
140 if (!bGroup.isNull())
142 if (Bottle *b=bGroup.find(
"reach_above_orientation").asList())
144 size_t len=std::min(o.length(),(
size_t)b->size());
145 for (
size_t i=0; i<len; i++)
146 o[i]=b->get(i).asFloat64();
151 yError()<<
"Unable to retrieve \"reach_above_orientation\" for "<<hand;
156 bool getObjectLocation(
const string &
object, Vector &x,
157 const bool doOutliersRemoval=
false)
161 int ack=Vocab32::encode(
"ack");
162 if (opcPort.getOutputCount()>0)
165 cmd.addVocab32(
"ask");
166 Bottle &content=cmd.addList();
167 Bottle &cond_1=content.addList();
168 cond_1.addString(
"entity");
169 cond_1.addString(
"==");
170 cond_1.addString(
"object");
171 content.addString(
"&&");
172 Bottle &cond_2=content.addList();
173 cond_2.addString(
"name");
174 cond_2.addString(
"==");
175 cond_2.addString(
object);
177 yInfo()<<
"querying opc: "<<cmd.toString();
178 opcPort.write(cmd,rep);
179 yInfo()<<
"opc response: "<<rep.toString();
182 if (rep.get(0).asVocab32()==ack)
184 if (Bottle *idField=rep.get(1).asList())
186 if (Bottle *idValues=idField->get(1).asList())
189 int id=idValues->get(0).asInt32();
190 cmd.addVocab32(
"get");
191 Bottle &content=cmd.addList();
192 Bottle &list_bid=content.addList();
193 list_bid.addString(
"id");
194 list_bid.addInt32(
id);
195 Bottle &list_propSet=content.addList();
196 list_propSet.addString(
"propSet");
197 list_propSet.addList().addString(
"position_3d");
199 deque<Vector> points;
200 size_t numCycles=(doOutliersRemoval?(size_t)objLocIter:1);
201 for (
size_t i=0; i<numCycles; i++)
204 yInfo()<<
"querying opc: "<<cmd.toString();
205 opcPort.write(cmd,rep);
206 yInfo()<<
"opc response: "<<rep.toString();
207 if (rep.get(0).asVocab32()==ack)
209 if (Bottle *propField=rep.get(1).asList())
211 if (Bottle *b=propField->find(
"position_3d").asList())
214 size_t len=std::min(x.length(),(
size_t)b->size());
215 for (
size_t j=0; j<len; j++)
216 p[j]=b->get(j).asFloat64();
231 if (points.size()>=numCycles)
233 x=(doOutliersRemoval?
234 removeOutliers(points):
245 yInfo()<<
"object location is ("<<x.toString(3,3)<<
")";
247 yError()<<
"Unable to retrieve object location";
253 void moveHandUp(
const string &hand)
255 ICartesianControl *icart;
256 Vector reachAboveOrientation;
259 drvCartLeft.view(icart);
260 reachAboveOrientation=reachAboveOrientationLeft;
264 drvCartRight.view(icart);
265 reachAboveOrientation=reachAboveOrientationRight;
269 icart->storeContext(&ctxt);
272 dof[0]=dof[1]=dof[2]=0.0;
273 icart->setDOF(dof,dof);
278 icart->goToPose(x,reachAboveOrientation);
279 icart->waitMotionDone(0.1,5.0);
281 icart->restoreContext(ctxt);
282 icart->deleteContext(ctxt);
286 Vector getHandLocation(
const string &hand)
288 ICartesianControl *icart;
290 drvCartLeft.view(icart);
292 drvCartRight.view(icart);
300 string composeEntry(
const string &hand,
const string &
object)
302 return (hand+
"-"+
object);
306 bool calibration_start(
const string &hand,
const string &
object,
310 if (arePort.getOutputCount()>0)
313 if (getObjectLocation(
object,x))
315 Bottle areCmd,areRep;
316 areCmd.addString(
"look");
317 areCmd.addList().read(x);
318 areCmd.addString(
"wait");
319 yInfo()<<
"looking at "<<
object<<
" in ("<<x.toString(3,3)<<
")";
320 arePort.write(areCmd,areRep);
321 if (areRep.get(0).asVocab32()==ACK)
323 if (getObjectLocation(
object,x,
true))
325 Bottle areCmd,areRep;
326 areCmd.addString(
"touch");
327 areCmd.addList().read(x);
328 areCmd.addString(hand);
329 areCmd.addString(
"still");
330 yInfo()<<
"touching "<<
object<<
" in ("<<x.toString(3,3)<<
") with "<<hand<<
" hand";
331 arePort.write(areCmd,areRep);
332 if (areRep.get(0).asVocab32()==ACK)
336 Bottle areCmd,areRep;
337 areCmd.addString(
"hand");
338 areCmd.addString(
"pretake_hand");
339 areCmd.addString(hand);
340 yInfo()<<
"moving hand";
341 arePort.write(areCmd,areRep);
342 if (areRep.get(0).asVocab32()==ACK)
344 Bottle areCmd,areRep;
345 areCmd.addString(
"calib");
346 areCmd.addString(
"kinematics");
347 areCmd.addString(
"start");
348 yInfo()<<
"starting calibration";
349 areCmd.addString(hand);
350 arePort.write(areCmd,areRep);
351 if (areRep.get(0).asVocab32()==ACK)
355 calibEntry=(entry.empty()?composeEntry(hand,
object):entry);
370 bool calibration_stop()
375 Vector x=getHandLocation(calibHand);
376 table[calibEntry].calibrator.addPoints(calibLoc,x);
377 table[calibEntry].calibrated=
false;
379 yInfo()<<
"pushing ("<<calibLoc.toString(5,5)<<
") ("
380 <<x.toString(5,5)<<
")";
382 if (arePort.getOutputCount()>0)
384 Bottle areCmd,areRep;
385 areCmd.addString(
"calib");
386 areCmd.addString(
"kinematics");
387 areCmd.addString(
"stop");
388 areCmd.addString(
"skip");
389 yInfo()<<
"stopping calibration";
390 arePort.write(areCmd,areRep);
391 if (areRep.get(0).asVocab32()==ACK)
393 Bottle areCmd,areRep;
394 areCmd.addString(
"home");
395 areCmd.addString(
"all");
397 arePort.write(areCmd,areRep);
398 reply=(areRep.get(0).asVocab32()==ACK);
409 bool calibration_clear(
const string &hand,
const string &
object,
412 string entry_name=(entry.empty()?composeEntry(hand,
object):entry);
413 map<string,TableEntry>::iterator it=table.find(entry_name);
424 vector<string> calibration_list()
426 vector<string> reply;
427 for (map<string,TableEntry>::iterator it=table.begin();
428 it!=table.end(); it++)
429 reply.push_back(it->first);
434 CalibPointReq get_location(
const string &hand,
const string &
object,
438 string entry_name=(entry.empty()?composeEntry(hand,
object):entry);
439 if (arePort.getOutputCount()>0)
442 if (getObjectLocation(
object,x))
444 Bottle areCmd,areRep;
445 areCmd.addString(
"look");
446 areCmd.addList().read(x);
447 areCmd.addString(
"wait");
448 yInfo()<<
"looking at "<<
object<<
" in ("<<x.toString(3,3)<<
")";
449 arePort.write(areCmd,areRep);
450 if (areRep.get(0).asVocab32()==ACK)
452 if (getObjectLocation(
object,x,
true))
455 map<string,TableEntry>::iterator it=table.find(entry_name);
458 TableEntry& entry=it->second;
459 if (!entry.calibrated)
461 if (entry.calibrator.getNumPoints()>2)
464 entry.calibrator.calibrate(entry.H, err);
465 yInfo()<<
"H=\n"<<entry.H.toString(5, 5);
466 yInfo()<<
"calibration error="<<err;
467 entry.calibrated=
true;
470 yError()<<
"Unable to calibrate: too few points";
473 if (entry.calibrated)
475 Vector res=x; res.push_back(1.0);
489 CalibPointReq get_location_nolook(
const string &entry,
const double x,
490 const double y,
const double z,
494 map<string,TableEntry>::iterator it=table.find(entry);
497 TableEntry &entry=it->second;
498 if (!entry.calibrated)
500 if (entry.calibrator.getNumPoints()>2)
503 entry.calibrator.calibrate(entry.H,err);
504 yInfo()<<
"H=\n"<<entry.H.toString(5,5);
505 yInfo()<<
"calibration error="<<err;
506 entry.calibrated=
true;
509 yError()<<
"Unable to calibrate: too few points";
512 if (entry.calibrated)
515 res[0]=x; res[1]=y; res[2]=z;
516 res=(invert?SE3inv(entry.H):entry.H)*res;
528 map<string,TableEntry>::iterator it=table.find(entry);
531 TableEntry &entry=it->second;
532 if (!entry.calibrated)
534 if (entry.calibrator.getNumPoints()>2)
537 entry.calibrator.calibrate(entry.H,err);
538 yInfo()<<
"H=\n"<<entry.H.toString(5,5);
539 yInfo()<<
"calibration error="<<err;
540 entry.calibrated=
true;
543 yError()<<
"Unable to calibrate: too few points";
546 if (entry.calibrated)
554 bool add_pair(
const string &entry,
const double xi,
555 const double yi,
const double zi,
556 const double xo,
const double yo,
560 in[0]=xi; in[1]=yi; in[2]=xi;
561 out[0]=xo; out[1]=yo; out[2]=xo;
562 table[entry].calibrator.addPoints(in,out);
563 table[entry].calibrated=
false;
570 string fileName=rf->getHomeContextPath()+
"/"+
571 rf->find(
"calibration-file").asString();
572 yInfo()<<
"Saving calibration in "<<fileName;
573 ofstream fout(fileName.c_str());
577 vector<string> entries=calibration_list();
578 for (
size_t i=0; i<entries.size(); i++)
579 fout<<entries[i]<<
" ";
580 fout<<
")"<<endl<<endl;
582 for (map<string,TableEntry>::iterator it=table.begin();
583 it!=table.end(); it++)
585 fout<<
"["<<it->first<<
"]"<<endl;
586 TableEntry &entry=it->second;
587 fout<<
"calibrated "<<(entry.calibrated?
"true":
"false")<<endl;
588 fout<<
"H ("<<entry.H.toString(5,5,
" ")<<
")"<<endl;
589 fout<<
"numPoints "<<entry.calibrator.getNumPoints()<<endl;
591 deque<Vector> in,out;
592 entry.calibrator.getPoints(in,out);
593 for (
size_t j=0; j<entry.calibrator.getNumPoints(); j++)
594 fout<<
"points_"<<j<<
" ("
595 <<in[j].toString(5,5)<<
") ("
596 <<out[j].toString(5,5)<<
")"
614 rf.setDefaultContext(this->rf->getContext().c_str());
615 rf.setDefaultConfigFile(this->rf->find(
"calibration-file").asString().c_str());
616 rf.configure(0,NULL);
618 if (Bottle *entries=rf.find(
"entries").asList())
620 for (
int i=0; i<entries->size(); i++)
622 string entry_name=entries->get(i).asString();
623 Bottle &bGroup=rf.findGroup(entry_name);
624 if (!bGroup.isNull())
626 table[entry_name].calibrated=(bGroup.check(
"calibrated",Value(
"false")).asString()==
"true");
627 if (Bottle *bH=bGroup.find(
"H").asList())
631 for (
int r=0; r<4; r++)
632 for (
int c=0; c<4; c++)
633 table[entry_name].H(r,c)=bH->get(4*r+c).asFloat64();
637 int numPoints=bGroup.check(
"numPoints",Value(0)).asInt32();
638 for (
int j=0; j<numPoints; j++)
640 ostringstream tag; tag<<
"points_"<<j;
641 Bottle &bPoints=bGroup.findGroup(tag.str());
642 if (bPoints.size()>=3)
644 Bottle *bIn=bPoints.get(1).asList();
645 Bottle *bOut=bPoints.get(2).asList();
646 if ((bIn!=NULL) && (bOut!=NULL))
649 size_t len_in=std::min(in.length(),(
size_t)bIn->size());
650 for (
size_t k=0; k<len_in; k++)
651 in[k]=bIn->get(k).asFloat64();
654 size_t len_out=std::min(out.length(),(
size_t)bOut->size());
655 for (
size_t k=0; k<len_out; k++)
656 out[k]=bOut->get(k).asFloat64();
658 table[entry_name].calibrator.addPoints(in,out);
673 yInfo()<<
"Table Content";
674 for (map<string,TableEntry>::iterator it=table.begin();
675 it!=table.end(); it++)
677 TableEntry &entry=it->second;
678 yInfo()<<
"["<<it->first<<
"]";
679 yInfo()<<
"calibrated "<<(entry.calibrated?
"true":
"false");
680 yInfo()<<
"H=\n"<<entry.H.toString(5,5);
682 deque<Vector> in,out;
683 entry.calibrator.getPoints(in,out);
684 for (
size_t i=0; i<entry.calibrator.getNumPoints(); i++)
685 yInfo()<<
"("<<in[i].toString(5,5)<<
") ("
686 <<out[i].toString(5,5)<<
")";
692 bool configure(ResourceFinder &rf)
695 string robot=rf.check(
"robot",Value(
"icub")).asString();
696 testModeOn=(rf.check(
"test-mode",Value(
"off")).asString()==
"on");
697 zOffset=rf.check(
"z-offset",Value(0.0)).asFloat64();
698 objLocIter=rf.check(
"object-location-iterations",Value(40)).asInt32();
700 ResourceFinder areRF;
701 areRF.setDefaultContext(rf.find(
"are-context").asString().c_str());
702 areRF.setDefaultConfigFile(rf.find(
"are-config-file").asString().c_str());
703 areRF.configure(0,NULL);
705 if (!getHandOrientation(areRF,
"left_arm",reachAboveOrientationLeft))
708 if (!getHandOrientation(areRF,
"right_arm",reachAboveOrientationRight))
713 Property option(
"(device cartesiancontrollerclient)");
714 option.put(
"remote",(
"/"+robot+
"/cartesianController/left_arm"));
715 option.put(
"local",(
"/"+getName(
"/cartesian/left_arm")).c_str());
716 if (!drvCartLeft.open(option))
718 yError()<<
"Unable to open cartesiancontrollerclient device for left_arm";
722 option.unput(
"remote"); option.unput(
"local");
723 option.put(
"remote",(
"/"+robot+
"/cartesianController/right_arm"));
724 option.put(
"local",(
"/"+getName(
"/cartesian/right_arm")).c_str());
725 if (!drvCartRight.open(option))
727 yError()<<
"Unable to open cartesiancontrollerclient device for right_arm";
733 arePort.open((
"/"+getName(
"/are")).c_str());
734 opcPort.open((
"/"+getName(
"/opc")).c_str());
735 rpcPort.open((
"/"+getName(
"/rpc")).c_str());
751 if (drvCartLeft.isValid())
753 if (drvCartRight.isValid())
754 drvCartRight.close();
775 int main(
int argc,
char *argv[])
778 if (!yarp.checkNetwork())
780 yError()<<
"YARP server not available!";
785 calib.setName(
"iolReachingCalibration");
788 rf.setDefaultContext(calib.getName().c_str());
789 rf.setDefault(
"calibration-file",
"calibration.ini");
790 rf.setDefault(
"are-context",
"actionsRenderingEngine");
791 rf.setDefault(
"are-config-file",
"config.ini");
792 rf.configure(argc,argv);
794 return calib.runModule(rf);
CalibMatrixReq IDL structure to ask for calibration matrix.
CalibPointReq IDL structure to send/receive points.
iolReachingCalibration_IDL IDL Interface to IOL Reaching Calibration services.