iol
main.cpp
1 /*
2  * Copyright (C) 2016 iCub Facility - 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 <cmath>
19 #include <algorithm>
20 #include <string>
21 #include <sstream>
22 #include <deque>
23 #include <map>
24 #include <set>
25 #include <fstream>
26 #include <iomanip>
27 
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>
33 
34 #include <iCub/ctrl/outliersDetection.h>
35 #include <iCub/optimization/calibReference.h>
36 
37 #include "src/iolReachingCalibration_IDL.h"
38 
39 #define ACK yarp::os::createVocab32('a','c','k')
40 
41 using namespace std;
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;
48 
49 
50 /********************************************************/
51 class Calibrator : public RFModule,
53 {
54  struct TableEntry
55  {
56  Matrix H;
57  bool calibrated;
58  CalibReferenceWithMatchedPoints calibrator;
59  TableEntry() : H(eye(4,4)), calibrated(false) { }
60  };
61  map<string,TableEntry> table;
62 
63  Vector reachAboveOrientationLeft;
64  Vector reachAboveOrientationRight;
65  double zOffset;
66  int objLocIter;
67 
68  ResourceFinder *rf;
69  bool testModeOn;
70  bool calibOngoing;
71  string calibHand;
72  string calibEntry;
73  Vector calibLoc;
74 
75  PolyDriver drvCartLeft;
76  PolyDriver drvCartRight;
77 
78  RpcClient arePort;
79  RpcClient opcPort;
80  RpcServer rpcPort;
81 
82  /********************************************************/
83  bool attach(RpcServer &source)
84  {
85  return this->yarp().attachAsServer(source);
86  }
87 
88  /********************************************************/
89  Vector removeOutliers(const deque<Vector> &points)
90  {
91  Vector x(3,0.0);
92  if (points.size()>0)
93  {
94  // average over the entire data
95  Vector avg(3,0.0);
96  for (size_t i=0; i<points.size(); i++)
97  avg+=points[i];
98  avg/=points.size();
99 
100  // compute the distances
101  Vector dist(points.size());
102  for (size_t i=0; i<points.size(); i++)
103  dist[i]=norm(points[i]-avg);
104 
105  // perform outliers removal
106  ModifiedThompsonTau detector;
107  set<size_t> outliers=detector.detect(dist,Property("(recursive)"));
108 
109  // average over inliers only
110  int cnt=0;
111  for (size_t i=0; i<points.size(); i++)
112  {
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");
117 
118  if (isInlier)
119  {
120  x+=points[i];
121  cnt++;
122  }
123  }
124 
125  if (cnt>0)
126  x/=cnt;
127  else
128  x=avg;
129  }
130 
131  return x;
132  }
133 
134  /********************************************************/
135  bool getHandOrientation(ResourceFinder &rf, const string &hand,
136  Vector &o)
137  {
138  o.resize(4,0.0);
139  Bottle &bGroup=rf.findGroup(hand);
140  if (!bGroup.isNull())
141  {
142  if (Bottle *b=bGroup.find("reach_above_orientation").asList())
143  {
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();
147  return true;
148  }
149  }
150 
151  yError()<<"Unable to retrieve \"reach_above_orientation\" for "<<hand;
152  return false;
153  }
154 
155  /*****************************************************/
156  bool getObjectLocation(const string &object, Vector &x,
157  const bool doOutliersRemoval=false)
158  {
159  bool ret=false;
160  x.resize(3,0.0);
161  int ack=Vocab32::encode("ack");
162  if (opcPort.getOutputCount()>0)
163  {
164  Bottle cmd,rep;
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);
176 
177  yInfo()<<"querying opc: "<<cmd.toString();
178  opcPort.write(cmd,rep);
179  yInfo()<<"opc response: "<<rep.toString();
180  if (rep.size()>1)
181  {
182  if (rep.get(0).asVocab32()==ack)
183  {
184  if (Bottle *idField=rep.get(1).asList())
185  {
186  if (Bottle *idValues=idField->get(1).asList())
187  {
188  Bottle cmd;
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");
198 
199  deque<Vector> points;
200  size_t numCycles=(doOutliersRemoval?(size_t)objLocIter:1);
201  for (size_t i=0; i<numCycles; i++)
202  {
203  Bottle rep;
204  yInfo()<<"querying opc: "<<cmd.toString();
205  opcPort.write(cmd,rep);
206  yInfo()<<"opc response: "<<rep.toString();
207  if (rep.get(0).asVocab32()==ack)
208  {
209  if (Bottle *propField=rep.get(1).asList())
210  {
211  if (Bottle *b=propField->find("position_3d").asList())
212  {
213  Vector p(3,0.0);
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();
217  points.push_back(p);
218  }
219  else
220  break;
221  }
222  else
223  break;
224  }
225  else
226  break;
227 
228  Time::delay(0.05);
229  }
230 
231  if (points.size()>=numCycles)
232  {
233  x=(doOutliersRemoval?
234  removeOutliers(points):
235  points.front());
236  ret=true;
237  }
238  }
239  }
240  }
241  }
242  }
243 
244  if (ret)
245  yInfo()<<"object location is ("<<x.toString(3,3)<<")";
246  else
247  yError()<<"Unable to retrieve object location";
248 
249  return ret;
250  }
251 
252  /********************************************************/
253  void moveHandUp(const string &hand)
254  {
255  ICartesianControl *icart;
256  Vector reachAboveOrientation;
257  if (hand=="left")
258  {
259  drvCartLeft.view(icart);
260  reachAboveOrientation=reachAboveOrientationLeft;
261  }
262  else
263  {
264  drvCartRight.view(icart);
265  reachAboveOrientation=reachAboveOrientationRight;
266  }
267 
268  int ctxt;
269  icart->storeContext(&ctxt);
270 
271  Vector dof(10,1.0);
272  dof[0]=dof[1]=dof[2]=0.0;
273  icart->setDOF(dof,dof);
274 
275  Vector x,o;
276  icart->getPose(x,o);
277  x[2]+=zOffset;
278  icart->goToPose(x,reachAboveOrientation);
279  icart->waitMotionDone(0.1,5.0);
280 
281  icart->restoreContext(ctxt);
282  icart->deleteContext(ctxt);
283  }
284 
285  /********************************************************/
286  Vector getHandLocation(const string &hand)
287  {
288  ICartesianControl *icart;
289  if (hand=="left")
290  drvCartLeft.view(icart);
291  else
292  drvCartRight.view(icart);
293 
294  Vector x,o;
295  icart->getPose(x,o);
296  return x;
297  }
298 
299  /********************************************************/
300  string composeEntry(const string &hand, const string &object)
301  {
302  return (hand+"-"+object);
303  }
304 
305  /********************************************************/
306  bool calibration_start(const string &hand, const string &object,
307  const string &entry)
308  {
309  bool reply=false;
310  if (arePort.getOutputCount()>0)
311  {
312  Vector x;
313  if (getObjectLocation(object,x))
314  {
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)
322  {
323  if (getObjectLocation(object,x,true))
324  {
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)
333  {
334  moveHandUp(hand);
335 
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)
343  {
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)
352  {
353  calibLoc=x;
354  calibHand=hand;
355  calibEntry=(entry.empty()?composeEntry(hand,object):entry);
356  calibOngoing=true;
357  reply=true;
358  }
359  }
360  }
361  }
362  }
363  }
364  }
365 
366  return reply;
367  }
368 
369  /********************************************************/
370  bool calibration_stop()
371  {
372  bool reply=false;
373  if (calibOngoing)
374  {
375  Vector x=getHandLocation(calibHand);
376  table[calibEntry].calibrator.addPoints(calibLoc,x);
377  table[calibEntry].calibrated=false;
378 
379  yInfo()<<"pushing ("<<calibLoc.toString(5,5)<<") ("
380  <<x.toString(5,5)<<")";
381 
382  if (arePort.getOutputCount()>0)
383  {
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)
392  {
393  Bottle areCmd,areRep;
394  areCmd.addString("home");
395  areCmd.addString("all");
396  yInfo()<<"homing";
397  arePort.write(areCmd,areRep);
398  reply=(areRep.get(0).asVocab32()==ACK);
399  }
400  }
401 
402  calibOngoing=false;
403  }
404 
405  return reply;
406  }
407 
408  /********************************************************/
409  bool calibration_clear(const string &hand, const string &object,
410  const string &entry)
411  {
412  string entry_name=(entry.empty()?composeEntry(hand,object):entry);
413  map<string,TableEntry>::iterator it=table.find(entry_name);
414  if (it!=table.end())
415  {
416  table.erase(it);
417  return true;
418  }
419  else
420  return false;
421  }
422 
423  /********************************************************/
424  vector<string> calibration_list()
425  {
426  vector<string> reply;
427  for (map<string,TableEntry>::iterator it=table.begin();
428  it!=table.end(); it++)
429  reply.push_back(it->first);
430  return reply;
431  }
432 
433  /********************************************************/
434  CalibPointReq get_location(const string &hand, const string &object,
435  const string &entry)
436  {
437  CalibPointReq reply("fail",0.0,0.0,0.0);
438  string entry_name=(entry.empty()?composeEntry(hand,object):entry);
439  if (arePort.getOutputCount()>0)
440  {
441  Vector x;
442  if (getObjectLocation(object,x))
443  {
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)
451  {
452  if (getObjectLocation(object,x,true))
453  {
454  reply=CalibPointReq("fail",x[0],x[1],x[2]);
455  map<string,TableEntry>::iterator it=table.find(entry_name);
456  if (it!=table.end())
457  {
458  TableEntry& entry=it->second;
459  if (!entry.calibrated)
460  {
461  if (entry.calibrator.getNumPoints()>2)
462  {
463  double err;
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;
468  }
469  else
470  yError()<<"Unable to calibrate: too few points";
471  }
472 
473  if (entry.calibrated)
474  {
475  Vector res=x; res.push_back(1.0);
476  res=entry.H*res;
477  reply=CalibPointReq("ok",res[0],res[1],res[2]);
478  }
479  }
480  }
481  }
482  }
483  }
484 
485  return reply;
486  }
487 
488  /********************************************************/
489  CalibPointReq get_location_nolook(const string &entry, const double x,
490  const double y, const double z,
491  const bool invert)
492  {
493  CalibPointReq reply("fail",x,y,z);
494  map<string,TableEntry>::iterator it=table.find(entry);
495  if (it!=table.end())
496  {
497  TableEntry &entry=it->second;
498  if (!entry.calibrated)
499  {
500  if (entry.calibrator.getNumPoints()>2)
501  {
502  double err;
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;
507  }
508  else
509  yError()<<"Unable to calibrate: too few points";
510  }
511 
512  if (entry.calibrated)
513  {
514  Vector res(4,1.0);
515  res[0]=x; res[1]=y; res[2]=z;
516  res=(invert?SE3inv(entry.H):entry.H)*res;
517  reply=CalibPointReq("ok",res[0],res[1],res[2]);
518  }
519  }
520 
521  return reply;
522  }
523 
524  /********************************************************/
525  CalibMatrixReq get_matrix(const string &entry)
526  {
527  CalibMatrixReq reply("fail",zeros(4,4));
528  map<string,TableEntry>::iterator it=table.find(entry);
529  if (it!=table.end())
530  {
531  TableEntry &entry=it->second;
532  if (!entry.calibrated)
533  {
534  if (entry.calibrator.getNumPoints()>2)
535  {
536  double err;
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;
541  }
542  else
543  yError()<<"Unable to calibrate: too few points";
544  }
545 
546  if (entry.calibrated)
547  reply=CalibMatrixReq("ok",entry.H);
548  }
549 
550  return reply;
551  }
552 
553  /********************************************************/
554  bool add_pair(const string &entry, const double xi,
555  const double yi, const double zi,
556  const double xo, const double yo,
557  const double zo)
558  {
559  Vector in(3),out(3);
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;
564  return true;
565  }
566 
567  /********************************************************/
568  bool save()
569  {
570  string fileName=rf->getHomeContextPath()+"/"+
571  rf->find("calibration-file").asString();
572  yInfo()<<"Saving calibration in "<<fileName;
573  ofstream fout(fileName.c_str());
574  if (fout.is_open())
575  {
576  fout<<"entries (";
577  vector<string> entries=calibration_list();
578  for (size_t i=0; i<entries.size(); i++)
579  fout<<entries[i]<<" ";
580  fout<<")"<<endl<<endl;
581 
582  for (map<string,TableEntry>::iterator it=table.begin();
583  it!=table.end(); it++)
584  {
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;
590 
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)<<")"
597  <<endl;
598  fout<<endl;
599  }
600 
601  fout.close();
602  return true;
603  }
604  else
605  return false;
606  }
607 
608  /********************************************************/
609  bool load()
610  {
611  table.clear();
612 
613  ResourceFinder rf;
614  rf.setDefaultContext(this->rf->getContext().c_str());
615  rf.setDefaultConfigFile(this->rf->find("calibration-file").asString().c_str());
616  rf.configure(0,NULL);
617 
618  if (Bottle *entries=rf.find("entries").asList())
619  {
620  for (int i=0; i<entries->size(); i++)
621  {
622  string entry_name=entries->get(i).asString();
623  Bottle &bGroup=rf.findGroup(entry_name);
624  if (!bGroup.isNull())
625  {
626  table[entry_name].calibrated=(bGroup.check("calibrated",Value("false")).asString()=="true");
627  if (Bottle *bH=bGroup.find("H").asList())
628  {
629  if (bH->size()==16)
630  {
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();
634  }
635  }
636 
637  int numPoints=bGroup.check("numPoints",Value(0)).asInt32();
638  for (int j=0; j<numPoints; j++)
639  {
640  ostringstream tag; tag<<"points_"<<j;
641  Bottle &bPoints=bGroup.findGroup(tag.str());
642  if (bPoints.size()>=3)
643  {
644  Bottle *bIn=bPoints.get(1).asList();
645  Bottle *bOut=bPoints.get(2).asList();
646  if ((bIn!=NULL) && (bOut!=NULL))
647  {
648  Vector in(3,0.0);
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();
652 
653  Vector out(3,0.0);
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();
657 
658  table[entry_name].calibrator.addPoints(in,out);
659  }
660  }
661  }
662  }
663  }
664  }
665 
666  print_table();
667  return true;
668  }
669 
670  /********************************************************/
671  void print_table()
672  {
673  yInfo()<<"Table Content";
674  for (map<string,TableEntry>::iterator it=table.begin();
675  it!=table.end(); it++)
676  {
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);
681  yInfo()<<"points:";
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)<<")";
687  }
688  }
689 
690 public:
691  /********************************************************/
692  bool configure(ResourceFinder &rf)
693  {
694  this->rf=&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();
699 
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);
704 
705  if (!getHandOrientation(areRF,"left_arm",reachAboveOrientationLeft))
706  return false;
707 
708  if (!getHandOrientation(areRF,"right_arm",reachAboveOrientationRight))
709  return false;
710 
711  if (!testModeOn)
712  {
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))
717  {
718  yError()<<"Unable to open cartesiancontrollerclient device for left_arm";
719  return false;
720  }
721 
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))
726  {
727  yError()<<"Unable to open cartesiancontrollerclient device for right_arm";
728  drvCartLeft.close();
729  return false;
730  }
731  }
732 
733  arePort.open(("/"+getName("/are")).c_str());
734  opcPort.open(("/"+getName("/opc")).c_str());
735  rpcPort.open(("/"+getName("/rpc")).c_str());
736  attach(rpcPort);
737 
738  calibOngoing=false;
739  load();
740 
741  return true;
742  }
743 
744  /********************************************************/
745  bool close()
746  {
747  rpcPort.close();
748  opcPort.close();
749  arePort.close();
750 
751  if (drvCartLeft.isValid())
752  drvCartLeft.close();
753  if (drvCartRight.isValid())
754  drvCartRight.close();
755 
756  save();
757  return true;
758  }
759 
760  /********************************************************/
761  double getPeriod()
762  {
763  return 1.0;
764  }
765 
766  /********************************************************/
767  bool updateModule()
768  {
769  return true;
770  }
771 };
772 
773 
774 /********************************************************/
775 int main(int argc, char *argv[])
776 {
777  Network yarp;
778  if (!yarp.checkNetwork())
779  {
780  yError()<<"YARP server not available!";
781  return 1;
782  }
783 
784  Calibrator calib;
785  calib.setName("iolReachingCalibration");
786 
787  ResourceFinder rf;
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);
793 
794  return calib.runModule(rf);
795 }
796 
797 
CalibMatrixReq IDL structure to ask for calibration matrix.
CalibPointReq IDL structure to send/receive points.
Definition: CalibPointReq.h:26
iolReachingCalibration_IDL IDL Interface to IOL Reaching Calibration services.