icub-client
main.cpp
Go to the documentation of this file.
1 /*
2  * Copyright (C) 2014 WYSIWYD Consortium, European Commission FP7 Project ICT-612139
3  * Authors: Stéphane Lallée
4  * email: stephane.lallee@gmail.com
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  * icub-client/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 <iostream>
19 #include <fstream>
20 #include <iomanip>
21 #include <map>
22 #include <string>
23 
24 #include <yarp/os/all.h>
25 #include <yarp/sig/all.h>
26 #include <yarp/math/Math.h>
27 
28 #include <iCub/ctrl/math.h>
29 #include <iCub/optimization/calibReference.h>
30 #include "icubclient/tags.h"
31 #include "icubclient/functions.h"
32 
33 using namespace std;
34 using namespace yarp::os;
35 using namespace yarp::sig;
36 using namespace yarp::math;
37 using namespace iCub::ctrl;
38 using namespace iCub::optimization;
39 using namespace icubclient;
40 
44 struct FrameInfo
45 {
46  string name;
47  bool isCalibrated;
48  Matrix H,HInv,S,SInv;
49  double calibrationError;
50  CalibReferenceWithMatchedPoints calibrator;
51 };
52 
56 class FrameHandlerModule: public RFModule
57 {
58  //Store the translation matrices allowing transformation from one frame toward the icub pivot frame
59  map<string, FrameInfo> frames;
60  string matricesFilePath;
61  bool isVerbose;
62  RpcServer rpc;
63  RpcClient opc;
64  Vector bboxMin,bboxMax;
65  Vector scaleMin,scaleMax;
66 
67 public:
68  /************************************************************************/
69  FrameHandlerModule() : isVerbose(false) {
70  ;
71  }
72 
73  bool configure(ResourceFinder &rf)
74  {
75  setName( rf.check("name",Value("referenceFrameHandler")).asString().c_str() );
76  isVerbose = (rf.check("isVerbose",Value(0)).asInt() == 1);
77  bool isEmpty = rf.check("empty")||rf.check("empty");
78 
79  //Define the working bounding box
80  bboxMin.resize(3); bboxMax.resize(3);
81  bboxMin[0]=-1.0; bboxMax[0]=1.0;
82  bboxMin[1]=-1.0; bboxMax[1]=1.0;
83  bboxMin[2]=-1.0; bboxMax[2]=1.0;
84 
85  //Define the scaling bounding box
86  scaleMin.resize(3); scaleMax.resize(3);
87  scaleMin[0]=0.1; scaleMax[0]=10.0;
88  scaleMin[1]=0.1; scaleMax[1]=10.0;
89  scaleMin[2]=1.0; scaleMax[2]=1.0;
90 
91  //Create the pivot reference
92  frames["icub"].name = "icub";
93  frames["icub"].isCalibrated = true;
94  frames["icub"].H = eye(4,4);
95  frames["icub"].S = eye(4,4);
96  frames["icub"].HInv = frames["icub"].H;
97  frames["icub"].SInv = frames["icub"].S;
98 
99  //Load matrices
100  string matricesFileName = rf.check("matricesFile",Value("frames.ini")).asString();
101  matricesFilePath = rf.findFile(matricesFileName);
102  Property matricesProp; matricesProp.fromConfigFile(matricesFilePath);
103  if (!isEmpty)
104  LoadMatrices(matricesProp);
105 
106  // we store new matrices in the home context path instead
107  matricesFilePath=rf.getHomeContextPath()+"/"+matricesFileName;
108 
109  opc.open("/" + getName() + "/opc:rpc"); //Create port to OPC
110  rpc.open("/" + getName() + "/rpc"); //Create RPC port
111  attach(rpc);
112 
113  return true;
114  }
115 
121  Matrix inverseScale(const Matrix &in)
122  {
123  Matrix out = in;
124  out(0,0) = 1.0 / out(0,0);
125  out(1,1) = 1.0 / out(1,1);
126  out(2,2) = 1.0 / out(2,2);
127  return out;
128  }
129 
133  void LoadMatrices(Property &prop)
134  {
135  Bottle &bFrames = prop.findGroup("frames");
136  for(unsigned int f=1; f<bFrames.size(); f+=3)
137  {
138  string currentFrame = bFrames.get(f).toString().c_str();
139  Bottle *bH = bFrames.get(f+1).asList();
140  Matrix H(4,4);
141  for(int i=0;i<4;i++)
142  {
143  for(int j=0;j<4;j++)
144  {
145  H(i,j)=bH->get(4*i+j).asDouble();
146  }
147  }
148 
149  Bottle *bS = bFrames.get(f+2).asList();
150  Matrix S(4,4);
151  for(int i=0;i<4;i++)
152  {
153  for(int j=0;j<4;j++)
154  {
155  S(i,j)=bS->get(4*i+j).asDouble();
156  }
157  }
158 
159  frames[currentFrame].name = currentFrame;
160  frames[currentFrame].H = H;
161  frames[currentFrame].HInv = SE3inv(frames[currentFrame].H);
162  frames[currentFrame].S = S;
163  frames[currentFrame].SInv = inverseScale(frames[currentFrame].S);
164  frames[currentFrame].isCalibrated = true;
165 
166  cout<<"Loading Frame"<<endl
167  <<frames[currentFrame].name<<endl
168  <<frames[currentFrame].H.toString(3,3)<<endl
169  <<frames[currentFrame].S.toString(3,3)<<endl;
170  }
171  }
172 
173 
178  void SaveMatrices(const string &fileName)
179  {
180  ofstream file(fileName.c_str());
181  file<<"[frames]"<<endl;
182  for(map<string, FrameInfo>::iterator it=frames.begin(); it!= frames.end(); it++)
183  {
184  if (it->second.isCalibrated)
185  {
186  file<<it->second.name<<endl;
187  Bottle b;
188  b.clear();
189 
190  for(int i=0;i<4;i++)
191  {
192  for(int j=0;j<4;j++)
193  {
194  b.addDouble(it->second.H(i,j));
195  }
196  }
197 
198  file<< b.toString().c_str()<<endl;
199 
200  Bottle bScale;
201  bScale.clear();
202  for(int i=0;i<4;i++)
203  {
204  for(int j=0;j<4;j++)
205  {
206  bScale.addDouble(it->second.S(i,j));
207  }
208  }
209  file<< bScale.toString().c_str()<<endl;
210 
211  cout<<"Saving Frame"<<endl
212  <<it->second.name<<endl
213  <<it->second.H.toString(3,3)<<endl
214  <<"Scale"<<endl
215  <<it->second.S.toString(3,3)<<endl;
216  }
217  }
218  file.close();
219  }
220 
225  void LoadMatricesFromOPC()
226  {
227  Bottle opcCmd,opcReply;
228  opcCmd.addVocab(Vocab::encode("ask"));
229  Bottle &query=opcCmd.addList();
230  query.addList().addString(ICUBCLIENT_OPC_FRAME_NAME);
231  opc.write(opcCmd,opcReply);
232 
233  Bottle ids=opcGetIdsFromAsk(opcReply);
234 
235  for(unsigned int i=0; i<ids.size() ; i++)
236  {
237  opcCmd.clear();
238  opcCmd.addVocab(Vocab::encode("get"));
239  Bottle &query=opcCmd.addList();
240  Bottle &idProp=query.addList();
241  idProp.addString("id");
242  idProp.addInt(ids.get(i).asInt());
243  Bottle &propSet=query.addList();
244  propSet.addString("propSet");
245  Bottle &props=propSet.addList();
246  props.addString(ICUBCLIENT_OPC_FRAME_NAME);
247  props.addString(ICUBCLIENT_OPC_FRAME_MATRIX);
248  props.addString(ICUBCLIENT_OPC_FRAME_SCALE);
249 
250  Bottle reply;
251  reply.clear();
252  opc.write(opcCmd,reply);
253 
254  if (reply.get(0).asVocab()==Vocab::encode("ack"))
255  {
256  if (Bottle *propList=reply.get(1).asList())
257  {
258  string currentFrame = propList->find(ICUBCLIENT_OPC_FRAME_NAME).asString().c_str();
259  Bottle *bH = propList->find(ICUBCLIENT_OPC_FRAME_MATRIX).asList();
260  Matrix H(4,4);
261  for(int i=0;i<4;i++)
262  {
263  for(int j=0;j<4;j++)
264  {
265  H(i,j)=bH->get(4*i+j).asDouble();
266  }
267  }
268 
269  Bottle *bS = propList->find(ICUBCLIENT_OPC_FRAME_SCALE).asList();
270  Matrix S(4,4);
271  for(int i=0;i<4;i++)
272  {
273  for(int j=0;j<4;j++)
274  {
275  S(i,j)=bS->get(4*i+j).asDouble();
276  }
277  }
278 
279  frames[currentFrame].name = currentFrame;
280  frames[currentFrame].H = H;
281  frames[currentFrame].HInv = SE3inv(frames[currentFrame].H);
282  frames[currentFrame].S = S;
283  frames[currentFrame].SInv = inverseScale(frames[currentFrame].S);
284  frames[currentFrame].isCalibrated = true;
285 
286  cout<<"Reading Frame from OPC"<<endl
287  <<frames[currentFrame].name<<endl
288  <<frames[currentFrame].H.toString(3,3)<<endl
289  <<frames[currentFrame].S.toString(3,3)<<endl;
290  }
291  }
292  }
293  }
294 
299  void SaveMatrices2OPC()
300  {
301  for(map<string, FrameInfo>::iterator it=frames.begin(); it!= frames.end(); it++)
302  {
303  if (it->second.isCalibrated)
304  {
305 
306  Bottle opcCmd,opcReply;
307  opcCmd.addVocab(Vocab::encode("ask"));
308  Bottle &query=opcCmd.addList();
309  Bottle& checkName = query.addList();
310  checkName.addString(ICUBCLIENT_OPC_FRAME_NAME);
311  checkName.addString("==");
312  checkName.addString(it->second.name.c_str());
313 
314  Bottle ids=opcGetIdsFromAsk(opcReply);
315 
316  opcCmd.clear();
317 
318  if (ids.size() == 0)
319  opcCmd.addVocab(Vocab::encode("add"));
320  else
321  opcCmd.addVocab(Vocab::encode("set"));
322 
323  Bottle &content = opcCmd.addList();
324  if (ids.size() != 0 )
325  {
326  Bottle &idUp = content.addList();
327  idUp.addString("id");
328  idUp.addInt(ids.get(0).asInt());
329  }
330 
331  Bottle &name = content.addList();
332  name.addString(ICUBCLIENT_OPC_FRAME_NAME);
333  name.addString(it->second.name.c_str());
334 
335  Bottle &matrix = content.addList();
336  matrix.addString(ICUBCLIENT_OPC_FRAME_MATRIX);
337  Bottle &bMat = matrix.addList();
338 
339  for(int i=0;i<4;i++)
340  {
341  for(int j=0;j<4;j++)
342  {
343  bMat.addDouble(it->second.H(i,j));
344  }
345  }
346 
347  Bottle &scale = content.addList();
348  scale.addString(ICUBCLIENT_OPC_FRAME_SCALE);
349  Bottle &bSca = matrix.addList();
350  for(int i=0;i<4;i++)
351  {
352  for(int j=0;j<4;j++)
353  {
354  bSca.addDouble(it->second.S(i,j));
355  }
356  }
357 
358  Bottle lastReply;
359  lastReply.clear();
360  opc.write(opcCmd,lastReply);
361  cout<<"Writing Frame 2 OPC"<<endl
362  <<it->second.name<<endl
363  <<it->second.H.toString(3,3)<<endl
364  <<"Scale"<<endl
365  <<it->second.S.toString(3,3)<<endl
366  <<lastReply.toString().c_str()<<endl;
367  }
368  }
369  }
370 
371  /************************************************************************/
372  bool respond(const Bottle& cmd, Bottle& reply)
373  {
374  //On the basis of the command sent an action is performed
375  switch (cmd.get(0).asVocab())
376  {
377  //Write matrices to opc - not supported!
378  case yarp::os::createVocab('o','p','c','w'):
379  {
380  SaveMatrices2OPC();
381  reply.addString("ack");
382  reply.addString("written to OPC");
383  reply.addString("this method is deprecated!");
384  break;
385  }
386 
387  //read matrices from opc - not supported!
388  case yarp::os::createVocab('o','p','c','r'):
389  {
390  LoadMatricesFromOPC();
391  reply.addString("ack");
392  reply.addString("updated from OPC");
393  reply.addString("this method is deprecated!");
394  break;
395  }
396 
397  //save matrices to file
398  case yarp::os::createVocab('s','a','v','e'):
399  {
400  SaveMatrices(matricesFilePath.c_str());
401  reply.addString("ack");
402  reply.addString("saved");
403  reply.addString( matricesFilePath.c_str() );
404  break;
405  }
406 
407  //Clear the list of points used for calibration
408  case yarp::os::createVocab('c','l','e','a'):
409  {
410  //Format is [clear] <frameName>
411  string frameName = cmd.get(1).asString().c_str();
412  std::cout<<"Clearing the points for frame : "<<frameName<<std::endl;
413  std::cout<<"Erasing "<<frames[frameName].calibrator.getNumPoints()<<std::endl;
414  //If the frame is not created yet we create it
415  if (frames.find(frameName) == frames.end() )
416  {
417  std::cout<<"Frame "<< frameName<<" was not present, creating it."<<std::endl;
418  frames[frameName].name = frameName;
419  frames[frameName].isCalibrated = false;
420  frames[frameName].calibrator.setBounds(bboxMin,bboxMax);
421  frames[frameName].calibrator.setScalingBounds(scaleMin,scaleMax);
422  frames[frameName].S = eye(4,4);
423  frames[frameName].H = eye(4,4);
424  frames[frameName].SInv = eye(4,4);
425  frames[frameName].HInv = eye(4,4);
426  }
427 
428  frames[frameName].calibrator.clearPoints();
429 
430  std::cout<<"After "<<frames[frameName].calibrator.getNumPoints()<<std::endl;
431  reply.addString("ack");
432  reply.addString("Cleared");
433  break;
434  }
435 
436  //Add a point to the list of points used for calibration
437  case yarp::os::createVocab('a','d','d'):
438  {
439  //Format is [add] <frameName> (x y z) (x' y' z')
440  string frameName = cmd.get(1).asString().c_str();
441 
442  //If the frame is not created yet we create it
443  if (frames.find(frameName) == frames.end() )
444  {
445  frames[frameName].name = frameName;
446  frames[frameName].isCalibrated = false;
447  frames[frameName].calibrator.setBounds(bboxMin,bboxMax);
448  frames[frameName].calibrator.setScalingBounds(scaleMin,scaleMax);
449  frames[frameName].S = eye(4,4);
450  frames[frameName].H = eye(4,4);
451  frames[frameName].SInv = eye(4,4);
452  frames[frameName].HInv = eye(4,4);
453  }
454 
455  //Add the point
456  Vector pOtherFrame(3), pPivotFrame(3);
457  for(int i=0; i<3; i++)
458  {
459  pOtherFrame[i] = cmd.get(2).asList()->get(i).asDouble();
460  pPivotFrame[i] = cmd.get(3).asList()->get(i).asDouble();
461  }
462  frames[frameName].calibrator.addPoints(pOtherFrame,pPivotFrame);
463  reply.addString("ack");
464  reply.addString("Point added");
465  break;
466  }
467 
468  //Calibrate a specific frame
469  case yarp::os::createVocab('c','a','l'):
470  {
471  string frameName = cmd.get(1).asString().c_str();
472 
473  //If the frame is not created yet it is an error
474  if (frames.find(frameName) == frames.end() )
475  {
476  reply.addString("nack");
477  reply.addString("Reference frame does not exist.");
478  break;
479  }
480  else
481  {
482  double t0=Time::now();
483  frames[frameName].calibrator.calibrate(frames[frameName].H,frames[frameName].calibrationError);
484  double dt=Time::now()-t0;
485  frames[frameName].HInv = SE3inv(frames[frameName].H);
486  frames[frameName].S=eye(4,4);
487  frames[frameName].SInv = inverseScale(frames[frameName].S);
488 
489 
490  cout<<"Asked for calibration of "<<frameName<<endl;
491  cout<<"H"<<endl<<frames[frameName].H.toString(3,3).c_str()<<endl;
492  cout<<"H^-1"<<endl<<frames[frameName].HInv.toString(3,3).c_str()<<endl;
493  cout<<"residual = "<<frames[frameName].calibrationError<<" [m]"<<endl;
494  cout<<"calibration performed in "<<dt<<" [s]"<<endl;
495  cout<<endl;
496  frames[frameName].isCalibrated = true;
497 
498  reply.addString("ack");
499  reply.addString(frames[frameName].H.toString());
500  }
501  break;
502  }
503 
504  //Calibrate a specific frame with scaling option
505  case yarp::os::createVocab('s','c','a','l'):
506  {
507  string frameName = cmd.get(1).asString().c_str();
508 
509  //If the frame is not created yet it is an error
510  if (frames.find(frameName) == frames.end() )
511  {
512  reply.addString("nack");
513  reply.addString("Reference frame does not exist.");
514  break;
515  }
516  else
517  {
518  Vector vScale(3);
519  double t0=Time::now();
520  frames[frameName].calibrator.calibrate(frames[frameName].H, vScale, frames[frameName].calibrationError);
521  double dt=Time::now()-t0;
522  frames[frameName].S = eye(4,4);
523  frames[frameName].S(0,0) = vScale(0);
524  frames[frameName].S(1,1) = vScale(1);
525  frames[frameName].S(2,2) = vScale(2);
526 
527  frames[frameName].HInv = SE3inv(frames[frameName].H);
528  frames[frameName].SInv = inverseScale(frames[frameName].S);
529 
530  cout<<"Asked for calibration of "<<frameName<<endl;
531  cout<<"H"<<endl<<frames[frameName].H.toString(3,3).c_str()<<endl;
532  cout<<"H^-1"<<endl<<frames[frameName].HInv.toString(3,3).c_str()<<endl;
533  cout<<"S"<<endl<<frames[frameName].S.toString(3,3).c_str()<<endl;
534  cout<<"S^-1"<<endl<<frames[frameName].SInv.toString(3,3).c_str()<<endl;
535  cout<<"residual = "<<frames[frameName].calibrationError<<" [m]"<<endl;
536  cout<<"calibration performed in "<<dt<<" [s]"<<endl;
537  cout<<endl;
538  frames[frameName].isCalibrated = true;
539  reply.addString("ack");
540  reply.addString(frames[frameName].H.toString());
541  }
542  break;
543  }
544 
545  //Calibrate a specific frame with isotropic scaling option
546  case yarp::os::createVocab('i','c','a','l'):
547  {
548  string frameName = cmd.get(1).asString().c_str();
549 
550  //If the frame is not created yet it is an error
551  if (frames.find(frameName) == frames.end() )
552  {
553  reply.addString("nack");
554  reply.addString("Reference frame does not exist.");
555  break;
556  }
557  else
558  {
559  double vScale;
560  double t0=Time::now();
561  frames[frameName].calibrator.calibrate(frames[frameName].H, vScale, frames[frameName].calibrationError);
562  double dt=Time::now()-t0;
563  frames[frameName].S = eye(4,4);
564  frames[frameName].S(0,0) = vScale;
565  frames[frameName].S(1,1) = vScale;
566  frames[frameName].S(2,2) = vScale;
567 
568  frames[frameName].HInv = SE3inv(frames[frameName].H);
569  frames[frameName].SInv = inverseScale(frames[frameName].S);
570 
571  cout<<"Asked for calibration of "<<frameName<<endl;
572  cout<<"H"<<endl<<frames[frameName].H.toString(3,3).c_str()<<endl;
573  cout<<"H^-1"<<endl<<frames[frameName].HInv.toString(3,3).c_str()<<endl;
574  cout<<"S"<<endl<<frames[frameName].S.toString(3,3).c_str()<<endl;
575  cout<<"S^-1"<<endl<<frames[frameName].SInv.toString(3,3).c_str()<<endl;
576  cout<<"residual = "<<frames[frameName].calibrationError<<" [m]"<<endl;
577  cout<<"calibration performed in "<<dt<<" [s]"<<endl;
578  cout<<endl;
579  frames[frameName].isCalibrated = true;
580 
581  reply.addString("ack");
582  reply.addString(frames[frameName].H.toString());
583  }
584  break;
585  }
586  //Transform one point from a specific reference frame to the pivot frame (icub)
587  case yarp::os::createVocab('t','r','a','n'):
588  {
589  string frameName1 = cmd.get(1).asString().c_str();
590  string frameName2 = cmd.get(2).asString().c_str();
591 
592  //If one of the frames is not created yet it is an error
593  if (frames.find(frameName1) == frames.end() || frames.find(frameName2) == frames.end() )
594  {
595  reply.addString("nack");
596  reply.addString("Reference frame does not exist.");
597  break;
598  }
599 
600  //If one of the frames is not calibrated yet it is an error
601  if (!frames[frameName1].isCalibrated || !frames[frameName2].isCalibrated )
602  {
603  reply.addString("nack");
604  reply.addString("Reference frame is not calibrated.");
605  break;
606  }
607 
608  Vector p0(4),p1(4),p2(4);
609  for(int i=0;i<3;i++)
610  p0[i] = cmd.get(3).asList()->get(i).asDouble();
611  p0[3] = 1;
612 
613  //Convert from frame1 to the pivot frame (iCub)
614  Matrix H = frames[frameName1].H;
615  Matrix S = frames[frameName1].S;
616  p1 = S*H*p0;
617 
618  //Convert from pivot (iCub) to the frame2
619  H = frames[frameName2].HInv;
620  S = frames[frameName2].SInv;
621  p2 = S*H*p1;
622 
623  if (isVerbose)
624  {
625  cout<<"Point in "<<frameName1<<" frame"<<p0.toString()<<endl;
626  cout<<"Point in iCub frame"<<p1.toString()<<endl;
627  cout<<"Point in "<<frameName2<<" frame"<<p2.toString()<<endl;
628  }
629  reply.addString("ack");
630  Bottle &bVect = reply.addList();
631  bVect.addDouble(p2[0]);bVect.addDouble(p2[1]);bVect.addDouble(p2[2]);
632  break;
633  }
634  //Set the transformation matrix from one frame to another, scaling is identity
635  case yarp::os::createVocab('s','m','a','t'):
636  {
637  string frameName1 = cmd.get(1).asString().c_str();
638  string frameName2 = cmd.get(2).asString().c_str();
639  cout<<"Received set matrix "<<frameName1<<" => "<<frameName2<<endl;
640 
641  if (frameName2 != "icub")
642  {
643  reply.addString("nack");
644  reply.addString("Can only set matrices to the icub pivot frame");
645  break;
646  }
647  frames[frameName1].name = frameName1;
648  frames[frameName1].isCalibrated = true;
649  frames[frameName1].H = eye(4,4);
650  frames[frameName1].S = eye(4,4);
651  Bottle *matrixList = cmd.get(3).asList();
652 
653  for(int i=0; i<4; i++)
654  {
655  for(int j=0; j<4; j++)
656  {
657  frames[frameName1].H(i,j) = matrixList->get(4*j+i).asDouble();
658  }
659  }
660  frames[frameName1].HInv = SE3inv(frames[frameName1].H);
661  frames[frameName1].SInv = inverseScale(frames[frameName1].S);
662  reply.addString("ack");
663 
664  cout<<"H"<<endl<<frames[frameName1].H.toString(3,3).c_str()<<endl;
665  cout<<"H^-1"<<endl<<frames[frameName1].HInv.toString(3,3).c_str()<<endl;
666  break;
667  }
668 
669  //Return the transformation matrix from one frame to another, incorporating scaling information
670  case yarp::os::createVocab('m','a','t'):
671  {
672  string frameName1 = cmd.get(1).asString().c_str();
673  string frameName2 = cmd.get(2).asString().c_str();
674 
675  //If one of the frames is not created yet it is an error
676  if (frames.find(frameName1) == frames.end() || frames.find(frameName2) == frames.end() )
677  {
678  reply.addString("nack");
679  reply.addString("Reference frame does not exist.");
680  break;
681  }
682 
683  //If one of the frames is not calibrated yet it is an error
684  if (!frames[frameName1].isCalibrated || !frames[frameName2].isCalibrated )
685  {
686  reply.addString("nack");
687  reply.addString("Reference frame is not calibrated.");
688  break;
689  }
690 
691  reply.addString("ack");
692  Matrix h1h2 = frames[frameName1].S*frames[frameName1].H * frames[frameName2].HInv*frames[frameName2].SInv;
693  std::cout<<std::endl<<std::endl;
694  std::cout<<frameName1<<" H : "<<frames[frameName1].H.toString(3,3)<<std::endl;
695  std::cout<<frameName1<<" S : "<<frames[frameName1].S.toString(3,3)<<std::endl;
696  std::cout<<frameName2<<" Hinv : "<<frames[frameName2].HInv.toString(3,3)<<std::endl;
697  std::cout<<frameName2<<" Sinv : "<<frames[frameName2].SInv.toString(3,3)<<std::endl;
698  std::cout<<"Result H : "<<h1h2.toString(3,3)<<std::endl;
699 
700  Bottle &matrixList = reply.addList();
701  for(int i=0; i<4; i++)
702  {
703  for(int j=0; j<4; j++)
704  {
705  matrixList.addDouble(h1h2(i,j));
706  }
707  }
708 
709  if (isVerbose)
710  {
711  cout<<"Retrieved H for "<<frameName1<<"-->"<<frameName2<<endl
712  <<h1h2.toString(3,3)<<endl
713  <<"Scale"<<endl;
714  }
715 
716  break;
717  }
718 
719  default: { reply.addString(getName()+" : unknown command."); return false; }
720  }
721 
722  return true;
723  }
724 
725  /************************************************************************/
726  bool interruptModule()
727  {
728  opc.interrupt();
729  rpc.interrupt();
730  return true;
731  }
732 
733  /************************************************************************/
734  bool close()
735  {
736  opc.interrupt();
737  opc.close();
738  rpc.interrupt();
739  rpc.close();
740  return true;
741  }
742 
743  /************************************************************************/
744  bool updateModule()
745  {
746  return true;
747  }
748 
749  /************************************************************************/
750  double getPeriod()
751  {
752  return 0.5;
753  }
754 };
755 
756 int main(int argc, char *argv[])
757 {
758  Network yarp;
759 
760  if (!yarp.checkNetwork())
761  return -1;
762 
763  ResourceFinder rf;
764  rf.setVerbose(true);
765  rf.setDefaultContext("referenceFrameHandler");
766  rf.setDefaultConfigFile("config.ini");
767  rf.configure(argc,argv);
768  FrameHandlerModule mod;
769 
770  return mod.runModule(rf);
771 }
ICUBCLIENT_OPC_FRAME_NAME
#define ICUBCLIENT_OPC_FRAME_NAME
Definition: tags.h:68
FrameHandlerModule::SaveMatrices2OPC
void SaveMatrices2OPC()
Saves the calibration matrices to the OPC.
Definition: main.cpp:299
drive_control_gui.argv
argv
Definition: drive_control_gui.py:6
FrameHandlerModule
Definition: main.cpp:56
FrameInfo::calibrator
CalibReferenceWithMatchedPoints calibrator
Definition: main.cpp:50
FrameHandlerModule::SaveMatrices
void SaveMatrices(const string &fileName)
Saves the matrices to a file; this file can later be loaded using LoadMatrices
Definition: main.cpp:178
FrameHandlerModule::LoadMatricesFromOPC
void LoadMatricesFromOPC()
Loads the calibration matrices from the OPC.
Definition: main.cpp:225
std
STL namespace.
FrameHandlerModule::inverseScale
Matrix inverseScale(const Matrix &in)
Inverts the scale of the provided matrix.
Definition: main.cpp:121
FrameInfo::SInv
Matrix SInv
Definition: main.cpp:48
FrameHandlerModule::configure
bool configure(ResourceFinder &rf)
Definition: main.cpp:73
FrameInfo::isCalibrated
bool isCalibrated
Definition: main.cpp:47
allostatic_plot.rf
rf
Definition: allostatic_plot.py:237
icubclient::opcGetIdsFromAsk
yarp::os::Bottle opcGetIdsFromAsk(const yarp::os::Bottle &reply)
Allow retrieving the list of unique identifiers of those items verifying the set of conditions querie...
Definition: functions.cpp:29
main
int main()
Definition: main.cpp:32
FrameInfo
Definition: main.cpp:44
FrameHandlerModule::FrameHandlerModule
FrameHandlerModule()
Definition: main.cpp:69
FrameHandlerModule::getPeriod
double getPeriod()
Definition: main.cpp:750
ICUBCLIENT_OPC_FRAME_MATRIX
#define ICUBCLIENT_OPC_FRAME_MATRIX
Definition: tags.h:69
allostatic_plot.mod
mod
Definition: allostatic_plot.py:236
FrameHandlerModule::LoadMatrices
void LoadMatrices(Property &prop)
LoadMatrices - reads the matrices from the specified matricesFile file.
Definition: main.cpp:133
FrameInfo::name
string name
Definition: main.cpp:46
FrameHandlerModule::close
bool close()
Definition: main.cpp:734
FrameInfo::calibrationError
double calibrationError
Definition: main.cpp:49
FrameHandlerModule::interruptModule
bool interruptModule()
Definition: main.cpp:726
FrameHandlerModule::respond
bool respond(const Bottle &cmd, Bottle &reply)
Definition: main.cpp:372
ICUBCLIENT_OPC_FRAME_SCALE
#define ICUBCLIENT_OPC_FRAME_SCALE
Definition: tags.h:70
FrameHandlerModule::updateModule
bool updateModule()
Definition: main.cpp:744
Generated on Wed Feb 19 2020 23:45:26 for icub-client by   doxygen 1.8.13