karma
•All Modules
main.cpp
1 /*
2  * Copyright (C) 2012 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 
101 #include <cstdio>
102 #include <mutex>
103 #include <algorithm>
104 #include <string>
105 #include <deque>
106 
107 #include <yarp/os/all.h>
108 #include <yarp/sig/all.h>
109 #include <yarp/dev/all.h>
110 #include <yarp/math/Math.h>
111 #include <yarp/cv/Cv.h>
112 
113 #include <IpTNLP.hpp>
114 #include <IpIpoptApplication.hpp>
115 
116 #include <opencv2/opencv.hpp>
117 
118 using namespace std;
119 using namespace yarp::os;
120 using namespace yarp::dev;
121 using namespace yarp::sig;
122 using namespace yarp::math;
123 using namespace yarp::cv;
124 
125 
126 /****************************************************************/
127 class FindToolTipNLP : public Ipopt::TNLP
128 {
129 protected:
130  const deque<Vector> &p;
131  const deque<Matrix> &H;
132 
133  Vector min;
134  Vector max;
135  Vector x0;
136  Vector x;
137 
138 public:
139  /****************************************************************/
140  FindToolTipNLP(const deque<Vector> &_p,
141  const deque<Matrix> &_H,
142  const Vector &_min, const Vector &_max) :
143  p(_p), H(_H)
144  {
145  min=_min;
146  max=_max;
147  x0=0.5*(min+max);
148  }
149 
150  /****************************************************************/
151  void set_x0(const Vector &x0)
152  {
153  size_t len=std::min(this->x0.length(),x0.length());
154  for (size_t i=0; i<len; i++)
155  this->x0[i]=x0[i];
156  }
157 
158  /****************************************************************/
159  Vector get_result() const
160  {
161  return x;
162  }
163 
164  /****************************************************************/
165  bool get_nlp_info(Ipopt::Index &n, Ipopt::Index &m, Ipopt::Index &nnz_jac_g,
166  Ipopt::Index &nnz_h_lag, IndexStyleEnum &index_style)
167  {
168  n=3;
169  m=nnz_jac_g=nnz_h_lag=0;
170  index_style=TNLP::C_STYLE;
171 
172  return true;
173  }
174 
175  /****************************************************************/
176  bool get_bounds_info(Ipopt::Index n, Ipopt::Number *x_l, Ipopt::Number *x_u,
177  Ipopt::Index m, Ipopt::Number *g_l, Ipopt::Number *g_u)
178  {
179  for (Ipopt::Index i=0; i<n; i++)
180  {
181  x_l[i]=min[i];
182  x_u[i]=max[i];
183  }
184 
185  return true;
186  }
187 
188  /****************************************************************/
189  bool get_starting_point(Ipopt::Index n, bool init_x, Ipopt::Number *x,
190  bool init_z, Ipopt::Number *z_L, Ipopt::Number *z_U,
191  Ipopt::Index m, bool init_lambda, Ipopt::Number *lambda)
192  {
193  for (Ipopt::Index i=0; i<n; i++)
194  x[i]=x0[i];
195 
196  return true;
197  }
198 
199  /****************************************************************/
200  bool eval_f(Ipopt::Index n, const Ipopt::Number *x, bool new_x,
201  Ipopt::Number &obj_value)
202  {
203  obj_value=0.0;
204  if (p.size()>0)
205  {
206  Vector _x(4);
207  _x[0]=x[0];
208  _x[1]=x[1];
209  _x[2]=x[2];
210  _x[3]=1.0;
211 
212  for (size_t i=0; i<p.size(); i++)
213  {
214  Vector pi=H[i]*_x;
215  pi=pi/pi[2];
216  pi.pop_back();
217 
218  obj_value+=norm2(p[i]-pi);
219  }
220 
221  obj_value/=p.size();
222  }
223 
224  return true;
225  }
226 
227  /****************************************************************/
228  bool eval_grad_f(Ipopt::Index n, const Ipopt::Number* x, bool new_x,
229  Ipopt::Number *grad_f)
230  {
231  Vector _x(4);
232  _x[0]=x[0];
233  _x[1]=x[1];
234  _x[2]=x[2];
235  _x[3]=1.0;
236 
237  grad_f[0]=grad_f[1]=grad_f[2]=0.0;
238  if (p.size()>0)
239  {
240  for (size_t i=0; i<p.size(); i++)
241  {
242  Vector pi=H[i]*_x;
243  pi=pi/pi[2];
244  pi.pop_back();
245 
246  Vector d=p[i]-pi;
247 
248  double u_num=dot(H[i].getRow(0),_x);
249  double v_num=dot(H[i].getRow(1),_x);
250 
251  double lambda=dot(H[i].getRow(2),_x);
252  double lambda2=lambda*lambda;
253 
254  Vector dp_dx1(2);
255  dp_dx1[0]=(H[i](0,0)*lambda-H[i](2,0)*u_num)/lambda2;
256  dp_dx1[1]=(H[i](1,0)*lambda-H[i](2,0)*v_num)/lambda2;
257 
258  Vector dp_dx2(2);
259  dp_dx2[0]=(H[i](0,1)*lambda-H[i](2,1)*u_num)/lambda2;
260  dp_dx2[1]=(H[i](1,1)*lambda-H[i](2,1)*v_num)/lambda2;
261 
262  Vector dp_dx3(2);
263  dp_dx3[0]=(H[i](0,2)*lambda-H[i](2,2)*u_num)/lambda2;
264  dp_dx3[1]=(H[i](1,2)*lambda-H[i](2,2)*v_num)/lambda2;
265 
266  grad_f[0]-=2.0*dot(d,dp_dx1);
267  grad_f[1]-=2.0*dot(d,dp_dx2);
268  grad_f[2]-=2.0*dot(d,dp_dx3);
269  }
270 
271  for (Ipopt::Index i=0; i<n; i++)
272  grad_f[i]/=p.size();
273  }
274 
275  return true;
276  }
277 
278  /****************************************************************/
279  bool eval_g(Ipopt::Index n, const Ipopt::Number *x, bool new_x,
280  Ipopt::Index m, Ipopt::Number *g)
281  {
282  return true;
283  }
284 
285  /****************************************************************/
286  bool eval_jac_g(Ipopt::Index n, const Ipopt::Number *x, bool new_x,
287  Ipopt::Index m, Ipopt::Index nele_jac, Ipopt::Index *iRow,
288  Ipopt::Index *jCol, Ipopt::Number *values)
289  {
290  return true;
291  }
292 
293 
294  /****************************************************************/
295  bool eval_h(Ipopt::Index n, const Ipopt::Number *x, bool new_x,
296  Ipopt::Number obj_factor, Ipopt::Index m, const Ipopt::Number *lambda,
297  bool new_lambda, Ipopt::Index nele_hess, Ipopt::Index *iRow,
298  Ipopt::Index *jCol, Ipopt::Number *values)
299  {
300  return true;
301  }
302 
303 
304  /****************************************************************/
305  void finalize_solution(Ipopt::SolverReturn status, Ipopt::Index n,
306  const Ipopt::Number *x, const Ipopt::Number *z_L,
307  const Ipopt::Number *z_U, Ipopt::Index m,
308  const Ipopt::Number *g, const Ipopt::Number *lambda,
309  Ipopt::Number obj_value, const Ipopt::IpoptData *ip_data,
310  Ipopt::IpoptCalculatedQuantities *ip_cq)
311  {
312  this->x.resize(n);
313  for (Ipopt::Index i=0; i<n; i++)
314  this->x[i]=x[i];
315  }
316 };
317 
318 
319 
320 /****************************************************************/
321 class FindToolTip
322 {
323 protected:
324  Vector min;
325  Vector max;
326  Vector x0;
327 
328  deque<Vector> p;
329  deque<Matrix> H;
330 
331  /****************************************************************/
332  double evalError(const Vector &x)
333  {
334  double error=0.0;
335  if (p.size()>0)
336  {
337  Vector _x=x;
338  if (_x.length()<4)
339  _x.push_back(1.0);
340 
341  for (size_t i=0; i<p.size(); i++)
342  {
343  Vector pi=H[i]*_x;
344  pi=pi/pi[2];
345  pi.pop_back();
346 
347  error+=norm(p[i]-pi);
348  }
349 
350  error/=p.size();
351  }
352 
353  return error;
354  }
355 
356 public:
357  /****************************************************************/
358  FindToolTip()
359  {
360  min.resize(3); max.resize(3);
361  min[0]=-1.0; max[0]=1.0;
362  min[1]=-1.0; max[1]=1.0;
363  min[2]=-1.0; max[2]=1.0;
364 
365  x0=0.5*(min+max);
366  }
367 
368  /****************************************************************/
369  void setBounds(const Vector &min, const Vector &max)
370  {
371  size_t len_min=std::min(this->min.length(),min.length());
372  size_t len_max=std::min(this->max.length(),max.length());
373 
374  for (size_t i=0; i<len_min; i++)
375  this->min[i]=min[i];
376 
377  for (size_t i=0; i<len_max; i++)
378  this->max[i]=max[i];
379  }
380 
381  /****************************************************************/
382  bool addItem(const Vector &pi, const Matrix &Hi)
383  {
384  if ((pi.length()>=2) && (Hi.rows()>=3) && (Hi.cols()>=4))
385  {
386  Vector _pi=pi.subVector(0,1);
387  Matrix _Hi=Hi.submatrix(0,2,0,3);
388 
389  p.push_back(_pi);
390  H.push_back(_Hi);
391 
392  return true;
393  }
394  else
395  return false;
396  }
397 
398  /****************************************************************/
399  void clearItems()
400  {
401  p.clear();
402  H.clear();
403  }
404 
405  /****************************************************************/
406  size_t getNumItems() const
407  {
408  return p.size();
409  }
410 
411  /****************************************************************/
412  bool setInitialGuess(const Vector &x0)
413  {
414  size_t len=std::min(x0.length(),this->x0.length());
415  for (size_t i=0; i<len; i++)
416  this->x0[i]=x0[i];
417 
418  return true;
419  }
420 
421  /****************************************************************/
422  bool solve(Vector &x, double &error)
423  {
424  if (p.size()>0)
425  {
426  Ipopt::SmartPtr<Ipopt::IpoptApplication> app=new Ipopt::IpoptApplication;
427  app->Options()->SetNumericValue("tol",1e-8);
428  app->Options()->SetIntegerValue("acceptable_iter",0);
429  app->Options()->SetStringValue("mu_strategy","adaptive");
430  app->Options()->SetIntegerValue("max_iter",300);
431  app->Options()->SetStringValue("nlp_scaling_method","gradient-based");
432  app->Options()->SetStringValue("hessian_approximation","limited-memory");
433  app->Options()->SetIntegerValue("print_level",0);
434  app->Options()->SetStringValue("derivative_test","none");
435  app->Initialize();
436 
437  Ipopt::SmartPtr<FindToolTipNLP> nlp=new FindToolTipNLP(p,H,min,max);
438 
439  nlp->set_x0(x0);
440  Ipopt::ApplicationReturnStatus status=app->OptimizeTNLP(GetRawPtr(nlp));
441 
442  x=nlp->get_result();
443  error=evalError(x);
444 
445  return (status==Ipopt::Solve_Succeeded);
446  }
447  else
448  return false;
449  }
450 };
451 
452 
453 
454 /************************************************************************/
455 class FinderModule: public RFModule, public PortReader
456 {
457 protected:
458  PolyDriver drvArmL;
459  PolyDriver drvArmR;
460  PolyDriver drvGaze;
461  ICartesianControl *iarm;
462  IGazeControl *igaze;
463  RpcServer rpcPort;
464  Matrix Prj;
465  mutex mtx;
466  FindToolTip solver;
467  Vector solution;
468  Bottle tip;
469  string arm;
470  string eye;
471  bool enabled;
472 
473  BufferedPort<ImageOf<PixelBgr> > imgInPort;
474  BufferedPort<ImageOf<PixelBgr> > imgOutPort;
475  Port dataInPort;
476  BufferedPort<Vector> logPort;
477 
478  /************************************************************************/
479  bool read(ConnectionReader &connection)
480  {
481  Bottle data;
482  data.read(connection);
483  if ((data.size()>=2) && enabled)
484  {
485  Vector xa,oa;
486  iarm->getPose(xa,oa);
487 
488  Vector xe,oe;
489  if (eye=="left")
490  igaze->getLeftEyePose(xe,oe);
491  else
492  igaze->getRightEyePose(xe,oe);
493 
494  Matrix Ha=axis2dcm(oa);
495  Ha.setSubcol(xa,0,3);
496 
497  Matrix He=axis2dcm(oe);
498  He.setSubcol(xe,0,3);
499 
500  Matrix H=Prj*SE3inv(He)*Ha;
501  Vector p(2);
502  p[0]=data.get(0).asDouble();
503  p[1]=data.get(1).asDouble();
504 
505  if (logPort.getOutputCount()>0)
506  {
507  Vector &log=logPort.prepare();
508 
509  log=p;
510  for (int i=0; i<H.rows(); i++)
511  log=cat(log,H.getRow(i));
512  for (int i=0; i<Prj.rows(); i++)
513  log=cat(log,Prj.getRow(i));
514  for (int i=0; i<Ha.rows(); i++)
515  log=cat(log,Ha.getRow(i));
516  for (int i=0; i<He.rows(); i++)
517  log=cat(log,He.getRow(i));
518 
519  logPort.write();
520  }
521 
522  mtx.lock();
523  solver.addItem(p,H);
524  mtx.unlock();
525  }
526 
527  return true;
528  }
529 
530 public:
531  /************************************************************************/
532  bool configure(ResourceFinder &rf)
533  {
534  string robot=rf.check("robot",Value("icub")).asString();
535  string name=rf.check("name",Value("karmaToolFinder")).asString();
536  arm=rf.check("arm",Value("right")).asString();
537  eye=rf.check("eye",Value("left")).asString();
538 
539  if ((arm!="left") && (arm!="right"))
540  {
541  printf("Invalid arm requested!\n");
542  return false;
543  }
544 
545  if ((eye!="left") && (eye!="right"))
546  {
547  printf("Invalid eye requested!\n");
548  return false;
549  }
550 
551  Property optionArmL("(device cartesiancontrollerclient)");
552  optionArmL.put("remote","/"+robot+"/cartesianController/left_arm");
553  optionArmL.put("local","/"+name+"/left_arm");
554  if (!drvArmL.open(optionArmL))
555  {
556  printf("Cartesian left_arm controller not available!\n");
557  terminate();
558  return false;
559  }
560 
561  Property optionArmR("(device cartesiancontrollerclient)");
562  optionArmR.put("remote","/"+robot+"/cartesianController/right_arm");
563  optionArmR.put("local","/"+name+"/right_arm");
564  if (!drvArmR.open(optionArmR))
565  {
566  printf("Cartesian right_arm controller not available!\n");
567  terminate();
568  return false;
569  }
570 
571  if (arm=="left")
572  drvArmL.view(iarm);
573  else
574  drvArmR.view(iarm);
575 
576  Property optionGaze("(device gazecontrollerclient)");
577  optionGaze.put("remote","/iKinGazeCtrl");
578  optionGaze.put("local","/"+name+"/gaze");
579  if (drvGaze.open(optionGaze))
580  drvGaze.view(igaze);
581  else
582  {
583  printf("Gaze controller not available!\n");
584  terminate();
585  return false;
586  }
587 
588  Bottle info;
589  igaze->getInfo(info);
590  if (Bottle *pB=info.find("camera_intrinsics_"+eye).asList())
591  {
592  int cnt=0;
593  Prj.resize(3,4);
594  for (int r=0; r<Prj.rows(); r++)
595  for (int c=0; c<Prj.cols(); c++)
596  Prj(r,c)=pB->get(cnt++).asDouble();
597  }
598  else
599  {
600  printf("Camera intrinsic parameters not available!\n");
601  terminate();
602  return false;
603  }
604 
605  imgInPort.open("/"+name+"/img:i");
606  imgOutPort.open("/"+name+"/img:o");
607  dataInPort.open("/"+name+"/in");
608  logPort.open("/"+name+"/log:o");
609  rpcPort.open("/"+name+"/rpc");
610  attach(rpcPort);
611 
612  Vector min(3),max(3);
613  min[0]=-1.0; max[0]=1.0;
614  min[1]=-1.0; max[1]=1.0;
615  min[2]=-1.0; max[2]=1.0;
616  solver.setBounds(min,max);
617  solution.resize(3,0.0);
618 
619  enabled=false;
620  dataInPort.setReader(*this);
621 
622  return true;
623  }
624 
625  /************************************************************************/
626  bool respond(const Bottle &command, Bottle &reply)
627  {
628  int ack=Vocab::encode("ack");
629  int nack=Vocab::encode("nack");
630 
631  if (command.size()>0)
632  {
633  switch (command.get(0).asVocab())
634  {
635  //-----------------
636  case createVocab('c','l','e','a'):
637  {
638  lock_guard<mutex> lg(mtx);
639  solver.clearItems();
640  solution=0.0;
641  reply.addVocab(ack);
642  return true;
643  }
644 
645  //-----------------
646  case createVocab('s','e','l','e'):
647  {
648  if (command.size()>=3)
649  {
650  string arm=command.get(1).asString();
651  string eye=command.get(2).asString();
652 
653  if ((arm=="left") || (arm=="right"))
654  this->arm=arm;
655 
656  if ((eye=="left") || (eye=="right"))
657  this->eye=eye;
658 
659  if (this->arm=="left")
660  drvArmL.view(iarm);
661  else
662  drvArmR.view(iarm);
663 
664  reply.addVocab(ack);
665  }
666  else
667  reply.addVocab(nack);
668 
669  return true;
670  }
671 
672  //-----------------
673  case createVocab('n','u','m'):
674  {
675  reply.addVocab(ack);
676 
677  lock_guard<mutex> lg(mtx);
678  reply.addInt((int)solver.getNumItems());
679  return true;
680  }
681 
682  //-----------------
683  case createVocab('f','i','n','d'):
684  {
685  double error;
686 
687  mtx.lock();
688  bool ok=solver.solve(solution,error);
689  mtx.unlock();
690 
691  if (ok)
692  {
693  reply.addVocab(ack);
694  for (size_t i=0; i<solution.length(); i++)
695  reply.addDouble(solution[i]);
696  }
697  else
698  reply.addVocab(nack);
699 
700  return true;
701  }
702 
703  //-----------------
704  case createVocab('s','h','o','w'):
705  {
706  if (command.size()>=4)
707  {
708  solution[0]=command.get(1).asDouble();
709  solution[1]=command.get(2).asDouble();
710  solution[2]=command.get(3).asDouble();
711 
712  reply.addVocab(ack);
713  }
714  else
715  reply.addVocab(nack);
716 
717  return true;
718  }
719 
720  //-----------------
721  case createVocab('t','i','p'):
722  {
723  if (tip.size()>=2)
724  {
725  reply.addVocab(ack);
726  reply.append(tip);
727  }
728  else
729  reply.addVocab(nack);
730 
731  return true;
732  }
733 
734  //-----------------
735  case createVocab('e','n','a','b'):
736  {
737  enabled=true;
738  reply.addVocab(ack);
739  return true;
740  }
741 
742  //-----------------
743  case createVocab('d','i','s','a'):
744  {
745  enabled=false;
746  reply.addVocab(ack);
747  return true;
748  }
749 
750  //-----------------
751  default:
752  return RFModule::respond(command,reply);
753  }
754  }
755 
756  reply.addVocab(nack);
757  return true;
758  }
759 
760  /************************************************************************/
761  double getPeriod()
762  {
763  return 0.03;
764  }
765 
766  /************************************************************************/
767  bool updateModule()
768  {
769  if (imgOutPort.getOutputCount()>0)
770  {
771  if (ImageOf<PixelBgr> *pImgBgrIn=imgInPort.read(false))
772  {
773  Vector xa,oa;
774  iarm->getPose(xa,oa);
775 
776  Matrix Ha=axis2dcm(oa);
777  Ha.setSubcol(xa,0,3);
778 
779  Vector v(4,0.0); v[3]=1.0;
780  Vector c=Ha*v;
781 
782  v=0.0; v[0]=0.05; v[3]=1.0;
783  Vector x=Ha*v;
784 
785  v=0.0; v[1]=0.05; v[3]=1.0;
786  Vector y=Ha*v;
787 
788  v=0.0; v[2]=0.05; v[3]=1.0;
789  Vector z=Ha*v;
790 
791  v=solution; v.push_back(1.0);
792  Vector t=Ha*v;
793 
794  Vector pc,px,py,pz,pt;
795  int camSel=(eye=="left")?0:1;
796  igaze->get2DPixel(camSel,c,pc);
797  igaze->get2DPixel(camSel,x,px);
798  igaze->get2DPixel(camSel,y,py);
799  igaze->get2DPixel(camSel,z,pz);
800  igaze->get2DPixel(camSel,t,pt);
801 
802  cv::Point point_c((int)pc[0],(int)pc[1]);
803  cv::Point point_x((int)px[0],(int)px[1]);
804  cv::Point point_y((int)py[0],(int)py[1]);
805  cv::Point point_z((int)pz[0],(int)pz[1]);
806  cv::Point point_t((int)pt[0],(int)pt[1]);
807 
808  cv::Mat imgMat=toCvMat(*pImgBgrIn);
809 
810  cv::circle(imgMat,point_c,4,cv::Scalar(0,255,0),4);
811  cv::circle(imgMat,point_t,4,cv::Scalar(255,0,0),4);
812 
813  cv::line(imgMat,point_c,point_x,cv::Scalar(0,0,255),2);
814  cv::line(imgMat,point_c,point_y,cv::Scalar(0,255,0),2);
815  cv::line(imgMat,point_c,point_z,cv::Scalar(255,0,0),2);
816  cv::line(imgMat,point_c,point_t,cv::Scalar(255,255,255),2);
817 
818  tip.clear();
819  tip.addInt(point_t.x);
820  tip.addInt(point_t.y);
821 
822  imgOutPort.prepare()=*pImgBgrIn;
823  imgOutPort.write();
824  }
825  }
826 
827  return true;
828  }
829 
830  /************************************************************************/
831  void terminate()
832  {
833 
834  imgInPort.close();
835  imgOutPort.close();
836  dataInPort.close(); // close prior to shutting down motor-interfaces
837  logPort.close();
838  rpcPort.close();
839 
840  if (drvArmL.isValid())
841  drvArmL.close();
842 
843  if (drvArmR.isValid())
844  drvArmR.close();
845 
846  if (drvGaze.isValid())
847  drvGaze.close();
848  }
849 
850  /************************************************************************/
851  bool close()
852  {
853  terminate();
854  return true;
855  }
856 };
857 
858 
859 
860 /****************************************************************/
861 int main(int argc, char *argv[])
862 {
863  Network yarp;
864  if (!yarp.checkNetwork())
865  {
866  printf("YARP server not available!\n");
867  return 1;
868  }
869 
870  ResourceFinder rf;
871  rf.configure(argc,argv);
872 
873  FinderModule mod;
874  return mod.runModule(rf);
875 }
876 
877 
Generated by doxygen 1.9.1