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 
107 #include <cstdio>
108 #include <string>
109 #include <algorithm>
110 
111 #include <yarp/os/all.h>
112 #include <yarp/dev/all.h>
113 #include <yarp/sig/all.h>
114 #include <yarp/math/Math.h>
115 
116 #include <iCub/ctrl/math.h>
117 
118 using namespace std;
119 using namespace yarp::os;
120 using namespace yarp::sig;
121 using namespace yarp::dev;
122 using namespace yarp::math;
123 using namespace iCub::ctrl;
124 
125 
126 /************************************************************************/
127 class KarmaMotor: public RFModule, public PortReader
128 {
129 protected:
130  PolyDriver driverG;
131  PolyDriver driverL;
132  PolyDriver driverR;
133  PolyDriver driverHL;
134  PolyDriver driverHR;
135 
136  IGazeControl *iGaze;
137  ICartesianControl *iCartCtrlL;
138  ICartesianControl *iCartCtrlR;
139  ICartesianControl *iCartCtrl;
140 
141  string pushHand;
142  Matrix toolFrame;
143 
144  string handUsed;
145  bool interrupting;
146  double flip_hand;
147  int shake_joint;
148 
149  bool elbow_set;
150  double elbow_height,elbow_weight;
151 
152  BufferedPort<Bottle> visionPort;
153  RpcClient finderPort;
154  RpcServer rpcPort;
155  Port stopPort;
156 
157  /************************************************************************/
158  double dist(const Matrix &M)
159  {
160  double ret=0.0;
161  for (int r=0; r<M.rows(); r++)
162  for (int c=0; c<M.cols(); c++)
163  ret+=M(r,c)*M(r,c);
164 
165  return sqrt(ret);
166  }
167 
168  /************************************************************************/
169  bool read(ConnectionReader &connection)
170  {
171  Bottle cmd; cmd.read(connection);
172  interruptModule();
173  return true;
174  }
175 
176  /************************************************************************/
177  bool respond(const Bottle &command, Bottle &reply)
178  {
179  int ack=Vocab::encode("ack");
180  int nack=Vocab::encode("nack");
181 
182  int cmd=command.get(0).asVocab();
183  switch (cmd)
184  {
185  //-----------------
186  case createVocab('p','u','s','h'):
187  {
188  Bottle payload=command.tail();
189  if (payload.size()>=5)
190  {
191  Vector c(3);
192  double theta;
193  double radius;
194 
195  c[0]=payload.get(0).asDouble();
196  c[1]=payload.get(1).asDouble();
197  c[2]=payload.get(2).asDouble();
198  theta=payload.get(3).asDouble();
199  radius=payload.get(4).asDouble();
200 
201  push(c,theta,radius,pushHand,toolFrame);
202  reply.addVocab(ack);
203  }
204 
205  break;
206  }
207 
208  //-----------------
209  case createVocab('d','r','a','w'):
210  case createVocab('v','d','r','a'):
211  {
212  Bottle payload=command.tail();
213  if (payload.size()>=6)
214  {
215  Vector c(3);
216  double theta;
217  double radius;
218  double dist;
219 
220  c[0]=payload.get(0).asDouble();
221  c[1]=payload.get(1).asDouble();
222  c[2]=payload.get(2).asDouble();
223  theta=payload.get(3).asDouble();
224  radius=payload.get(4).asDouble();
225  dist=payload.get(5).asDouble();
226 
227  double res=draw(cmd==createVocab('v','d','r','a'),c,theta,
228  radius,dist,pushHand,toolFrame);
229 
230  reply.addVocab(ack);
231  if (cmd==createVocab('v','d','r','a'))
232  reply.addDouble(res);
233  }
234 
235  break;
236  }
237 
238  //-----------------
239  case createVocab('f','i','n','d'):
240  {
241  Bottle payload=command.tail();
242  if (payload.size()>=2)
243  {
244  string arm=payload.get(0).asString();
245  string eye=payload.get(1).asString();
246  Bottle solution;
247 
248  if (findToolTip(arm,eye,solution))
249  {
250  reply.addVocab(ack);
251  reply.append(solution.tail());
252  }
253  else
254  reply.addVocab(nack);
255  }
256 
257  break;
258  }
259 
260  //-----------------
261  case createVocab('t','o','o','l'):
262  {
263  if (command.size()>1)
264  {
265  Bottle subcommand=command.tail();
266  int tag=subcommand.get(0).asVocab();
267  if (tag==Vocab::encode("attach"))
268  {
269  Bottle payload=subcommand.tail();
270  if (payload.size()>=4)
271  {
272  pushHand=payload.get(0).asString();
273 
274  Vector point(4);
275  point[0]=payload.get(1).asDouble();
276  point[1]=payload.get(2).asDouble();
277  point[2]=payload.get(3).asDouble();
278  point[3]=1.0;
279 
280  Vector r(4,0.0);
281  r[2]=-1.0;
282  r[3]=atan2(-point[1],point[0]);
283  toolFrame=axis2dcm(r);
284  toolFrame.setCol(3,point);
285 
286  reply.addVocab(ack);
287  }
288  }
289  else if (tag==Vocab::encode("get"))
290  {
291  reply.addVocab(ack);
292  reply.addString(pushHand);
293  reply.addDouble(toolFrame(0,3));
294  reply.addDouble(toolFrame(1,3));
295  reply.addDouble(toolFrame(2,3));
296  }
297  else if (tag==Vocab::encode("remove"))
298  {
299  pushHand="selectable";
300  toolFrame=eye(4,4);
301 
302  reply.addVocab(ack);
303  }
304  }
305 
306  break;
307  }
308 
309  //-----------------
310  default:
311  interrupting=false;
312  return RFModule::respond(command,reply);
313  }
314 
315  interrupting=false;
316  return true;
317  }
318 
319  /***************************************************************/
320  void changeElbowHeight()
321  {
322  if (elbow_set)
323  {
324  Bottle tweakOptions;
325  Bottle &optTask2=tweakOptions.addList();
326  optTask2.addString("task_2");
327  Bottle &plTask2=optTask2.addList();
328  plTask2.addInt(6);
329  Bottle &posPart=plTask2.addList();
330  posPart.addDouble(0.0);
331  posPart.addDouble(0.0);
332  posPart.addDouble(elbow_height);
333  Bottle &weightsPart=plTask2.addList();
334  weightsPart.addDouble(0.0);
335  weightsPart.addDouble(0.0);
336  weightsPart.addDouble(elbow_weight);
337  iCartCtrl->tweakSet(tweakOptions);
338  }
339  }
340 
341  /************************************************************************/
342  void push(const Vector &c, const double theta, const double radius,
343  const string &armType="selectable", const Matrix &frame=eye(4,4))
344  {
345  // wrt root frame: frame centered at c with x-axis pointing rightward,
346  // y-axis pointing forward and z-axis pointing upward
347  Matrix H0(4,4); H0.zero();
348  H0(1,0)=1.0;
349  H0(0,1)=-1.0;
350  H0(2,2)=1.0;
351  H0(0,3)=c[0]; H0(1,3)=c[1]; H0(2,3)=c[2]; H0(3,3)=1.0;
352 
353  double theta_rad=CTRL_DEG2RAD*theta;
354  double _c=cos(theta_rad);
355  double _s=sin(theta_rad);
356  double _theta=CTRL_RAD2DEG*atan2(_s,_c); // to have theta in [-180.0,180.0]
357  double epsilon=0.05;
358 
359  // wrt H0 frame: frame centered at R*[_c,_s] with z-axis pointing inward
360  // and x-axis tangential
361  Matrix H1(4,4); H1.zero();
362  H1(0,0)=-_s; H1(1,0)=_c;
363  H1(2,1)=-1.0;
364  H1(0,2)=-_c; H1(1,2)=-_s;
365  H1(0,3)=radius*_c; H1(1,3)=radius*_s; H1(3,3)=1.0;
366 
367  // wrt H0 frame: frame centered at R*[_c,_s] with z-axis pointing outward
368  // and x-axis tangential
369  Matrix H2(4,4); H2.zero();
370  H2(0,0)=_s; H2(1,0)=-_c;
371  H2(2,1)=-1.0;
372  H2(0,2)=_c; H2(1,2)=_s;
373  H2(0,3)=radius*_c; H2(1,3)=radius*_s; H2(3,3)=1.0;
374 
375  // matrices that serve to account for pushing with the back of the hand
376  Matrix H1eps=H1; Matrix H2eps=H2;
377  H1eps(0,3)+=epsilon*_c; H1eps(1,3)+=epsilon*_s;
378  H2eps(0,3)+=epsilon*_c; H2eps(1,3)+=epsilon*_s;
379 
380  // go back into root frame and apply tool (if any)
381  Matrix invFrame=SE3inv(frame);
382  H1=H0*H1*invFrame;
383  H2=H0*H2*invFrame;
384  H1eps=H0*H1eps*invFrame;
385  H2eps=H0*H2eps*invFrame;
386 
387  Vector xd1=H1.getCol(3).subVector(0,2);
388  Vector od1=dcm2axis(H1);
389 
390  Vector xd2=H2.getCol(3).subVector(0,2);
391  Vector od2=dcm2axis(H2);
392 
393  Vector xd1eps=H1eps.getCol(3).subVector(0,2);
394  Vector od1eps=dcm2axis(H1eps);
395 
396  Vector xd2eps=H2eps.getCol(3).subVector(0,2);
397  Vector od2eps=dcm2axis(H2eps);
398 
399  printf("identified locations...\n");
400  printf("xd1=(%s) od1=(%s)\n",xd1.toString(3,3).c_str(),od1.toString(3,3).c_str());
401  printf("xd2=(%s) od2=(%s)\n",xd2.toString(3,3).c_str(),od2.toString(3,3).c_str());
402 
403  // choose the arm
404  if (armType=="selectable")
405  {
406  if (xd1[1]>=0.0)
407  iCartCtrl=iCartCtrlR;
408  else
409  iCartCtrl=iCartCtrlL;
410  }
411  else if (armType=="left")
412  iCartCtrl=iCartCtrlL;
413  else
414  iCartCtrl=iCartCtrlR;
415 
416  // deal with the arm context
417  int context;
418  iCartCtrl->storeContext(&context);
419 
420  Bottle options;
421  Bottle &straightOpt=options.addList();
422  straightOpt.addString("straightness");
423  straightOpt.addDouble(10.0);
424  iCartCtrl->tweakSet(options);
425  changeElbowHeight();
426 
427  Vector dof;
428  iCartCtrl->getDOF(dof);
429 
430  dof=1.0; dof[1]=0.0;
431  iCartCtrl->setDOF(dof,dof);
432 
433  Vector xdhat1,odhat1,xdhat2,odhat2;
434  Vector dummy;
435 
436  // try out different poses
437  iCartCtrl->askForPose(xd1,od1,xdhat1,odhat1,dummy);
438  iCartCtrl->askForPose(xd2,od2,xdhat2,odhat2,dummy);
439 
440  Matrix Hhat1=axis2dcm(odhat1); Hhat1(0,3)=xdhat1[0]; Hhat1(1,3)=xdhat1[1]; Hhat1(2,3)=xdhat1[2];
441  Matrix Hhat2=axis2dcm(odhat2); Hhat2(0,3)=xdhat2[0]; Hhat2(1,3)=xdhat2[1]; Hhat2(2,3)=xdhat2[2];
442 
443  double d1=dist(H1-Hhat1);
444  double d2=dist(H2-Hhat2);
445 
446  printf("solutions...\n");
447  printf("#1: xdhat1=(%s) odhat1=(%s); e=%.3f\n",xdhat1.toString(3,3).c_str(),odhat1.toString(3,3).c_str(),d1);
448  printf("#2: xdhat2=(%s) odhat2=(%s); e=%.3f\n",xdhat2.toString(3,3).c_str(),odhat2.toString(3,3).c_str(),d2);
449  printf("selection: ");
450 
451  // compare solutions and choose the best
452  Vector *xd,*od;
453  if (fabs(_theta-90.0)<45.0)
454  {
455  printf("(detected singularity) ");
456  if (iCartCtrl==iCartCtrlR)
457  {
458  xd=&xd1;
459  od=&od1;
460  }
461  else
462  {
463  xd=&xd2;
464  od=&od2;
465  }
466  }
467  else if (fabs(_theta+90.0)<45.0)
468  {
469  printf("(detected singularity) ");
470  if (iCartCtrl==iCartCtrlR)
471  {
472  xd=&xd2;
473  od=&od2;
474  }
475  else
476  {
477  xd=&xd1;
478  od=&od1;
479  }
480  }
481  else if (d1<d2)
482  {
483  xd=&xd1;
484  od=&od1;
485  }
486  else
487  {
488  xd=&xd2;
489  od=&od2;
490  }
491 
492  if (xd==&xd1)
493  printf("#1 ");
494  else
495  printf("#2 ");
496 
497  if ((iCartCtrl==iCartCtrlR) && (_theta<0.0) && (xd==&xd2))
498  {
499  printf("(increased radius)");
500  xd=&xd2eps;
501  od=&od2eps;
502  }
503  else if ((iCartCtrl==iCartCtrlL) && (_theta<0.0) && (xd==&xd1))
504  {
505  printf("(increased radius)");
506  xd=&xd1eps;
507  od=&od1eps;
508  }
509 
510  printf(": xd=(%s); od=(%s)\n",xd->toString(3,3).c_str(),od->toString(3,3).c_str());
511 
512  // execute the movement
513  Vector offs(3,0.0); offs[2]=0.1;
514  if (!interrupting)
515  {
516  Vector x=*xd+offs;
517 
518  printf("moving to: x=(%s); o=(%s)\n",x.toString(3,3).c_str(),od->toString(3,3).c_str());
519  iCartCtrl->goToPoseSync(x,*od,1.0);
520  iCartCtrl->waitMotionDone(0.1,4.0);
521  }
522 
523  if (!interrupting)
524  {
525  printf("moving to: x=(%s); o=(%s)\n",xd->toString(3,3).c_str(),od->toString(3,3).c_str());
526  iCartCtrl->goToPoseSync(*xd,*od,1.0);
527  iCartCtrl->waitMotionDone(0.1,4.0);
528  }
529 
530  double rmin,rmax,tmin,tmax;
531  if (((fabs(theta)<10.0) || (fabs(theta-180.0)<10.0)))
532  {
533  rmin=0.04; rmax=0.18;
534  tmin=0.40; tmax=0.60;
535  }
536  else
537  {
538  rmin=0.04; rmax=0.18;
539  tmin=0.50; tmax=0.80;
540  }
541 
542  // safe guard for using the tool
543  if (armType!="selectable")
544  {
545  tmin*=1.3;
546  tmax*=1.3;
547  }
548 
549  double trajTime=tmin+((tmax-tmin)/(rmax-rmin))*(radius-rmin);
550  trajTime=std::max(std::min(tmax,trajTime),tmin);
551 
552  if (!interrupting)
553  {
554  Matrix H=axis2dcm(*od);
555  Vector center=c; center.push_back(1.0);
556  H.setCol(3,center);
557  Vector x=-1.0*frame.getCol(3); x[3]=1.0;
558  x=H*x; x.pop_back();
559 
560  printf("moving to: x=(%s); o=(%s)\n",x.toString(3,3).c_str(),od->toString(3,3).c_str());
561  iCartCtrl->goToPoseSync(x,*od,trajTime);
562  iCartCtrl->waitMotionDone(0.1,3.0);
563  }
564 
565  if (!interrupting)
566  {
567  printf("moving to: x=(%s); o=(%s)\n",xd->toString(3,3).c_str(),od->toString(3,3).c_str());
568  iCartCtrl->goToPoseSync(*xd,*od,1.0);
569  iCartCtrl->waitMotionDone(0.1,2.0);
570  }
571 
572  iCartCtrl->restoreContext(context);
573  iCartCtrl->deleteContext(context);
574  }
575 
576  /************************************************************************/
577  double draw(bool simulation, const Vector &c, const double theta, const double radius,
578  const double dist, const string &armType, const Matrix &frame=eye(4,4))
579  {
580  // c0 is the projection of c on the sagittal plane
581  Vector c_sag=c;
582  c_sag[1]=0.0;
583 
584  // wrt root frame: frame centered at c_sag with x-axis pointing rightward,
585  // y-axis pointing forward and z-axis pointing upward
586  Matrix H0(4,4); H0.zero();
587  H0(1,0)=1.0;
588  H0(0,1)=-1.0;
589  H0(2,2)=1.0;
590  H0(0,3)=c_sag[0]; H0(1,3)=c_sag[1]; H0(2,3)=c_sag[2]; H0(3,3)=1.0;
591 
592  double theta_rad=CTRL_DEG2RAD*theta;
593  double _c=cos(theta_rad);
594  double _s=sin(theta_rad);
595 
596  // wrt H0 frame: frame translated in R*[_c,_s]
597  Matrix H1=eye(4,4);
598  H1(0,3)=radius*_c; H1(1,3)=radius*_s;
599 
600  // wrt H1 frame: frame translated in [0,-dist]
601  Matrix H2=eye(4,4);
602  H2(1,3)=-dist;
603 
604  // go back into root frame
605  H2=H0*H1*H2;
606  H1=H0*H1;
607 
608  // apply final axes
609  Matrix R(3,3); R.zero();
610  R(0,0)=-1.0;
611  R(2,1)=-1.0;
612  R(1,2)=-1.0;
613 
614  H1.setSubmatrix(R,0,0);
615  H2.setSubmatrix(R,0,0);
616 
617  Vector xd1=H1.getCol(3).subVector(0,2);
618  Vector od1=dcm2axis(H1);
619 
620  Vector xd2=H2.getCol(3).subVector(0,2);
621  Vector od2=dcm2axis(H2);
622 
623  printf("identified locations on the sagittal plane...\n");
624  printf("xd1=(%s) od1=(%s)\n",xd1.toString(3,3).c_str(),od1.toString(3,3).c_str());
625  printf("xd2=(%s) od2=(%s)\n",xd2.toString(3,3).c_str(),od2.toString(3,3).c_str());
626 
627  // choose the arm
628  if (armType=="selectable")
629  {
630  if (xd1[1]>=0.0)
631  iCartCtrl=iCartCtrlR;
632  else
633  iCartCtrl=iCartCtrlL;
634  }
635  else if (armType=="left")
636  iCartCtrl=iCartCtrlL;
637  else
638  iCartCtrl=iCartCtrlR;
639 
640  // recover the original place: do translation and rotation
641  if (c[1]!=0.0)
642  {
643  Vector r(4,0.0);
644  r[2]=-1.0;
645  r[3]=atan2(c[1],fabs(c[0]));
646  Matrix H=axis2dcm(r);
647 
648  H(0,3)=H1(0,3);
649  H(1,3)=H1(1,3)+c[1];
650  H(2,3)=H1(2,3);
651  H1(0,3)=H1(1,3)=H1(2,3)=0.0;
652  H1=H*H1;
653 
654  H(0,3)=H2(0,3);
655  H(1,3)=H2(1,3)+c[1];
656  H(2,3)=H2(2,3);
657  H2(0,3)=H2(1,3)=H2(2,3)=0.0;
658  H2=H*H2;
659 
660  xd1=H1.getCol(3).subVector(0,2);
661  od1=dcm2axis(H1);
662 
663  xd2=H2.getCol(3).subVector(0,2);
664  od2=dcm2axis(H2);
665  }
666 
667  printf("in-place locations...\n");
668  printf("xd1=(%s) od1=(%s)\n",xd1.toString(3,3).c_str(),od1.toString(3,3).c_str());
669  printf("xd2=(%s) od2=(%s)\n",xd2.toString(3,3).c_str(),od2.toString(3,3).c_str());
670 
671  // apply tool (if any)
672  Matrix invFrame=SE3inv(frame);
673  H1=H1*invFrame;
674  H2=H2*invFrame;
675 
676  xd1=H1.getCol(3).subVector(0,2);
677  od1=dcm2axis(H1);
678 
679  xd2=H2.getCol(3).subVector(0,2);
680  od2=dcm2axis(H2);
681 
682  printf("apply tool (if any)...\n");
683  printf("xd1=(%s) od1=(%s)\n",xd1.toString(3,3).c_str(),od1.toString(3,3).c_str());
684  printf("xd2=(%s) od2=(%s)\n",xd2.toString(3,3).c_str(),od2.toString(3,3).c_str());
685 
686  // deal with the arm context
687  int context;
688  iCartCtrl->storeContext(&context);
689 
690  Bottle options;
691  Bottle &straightOpt=options.addList();
692  straightOpt.addString("straightness");
693  straightOpt.addDouble(30.0);
694  iCartCtrl->tweakSet(options);
695  changeElbowHeight();
696 
697  Vector dof;
698  iCartCtrl->getDOF(dof);
699 
700  dof=1.0; dof[1]=0.0;
701  iCartCtrl->setDOF(dof,dof);
702 
703  double res=0.0;
704 
705  // simulate the movements
706  if (simulation)
707  {
708  Vector xdhat1,odhat1,xdhat2,odhat2,qdhat;
709  iCartCtrl->askForPose(xd1,od1,xdhat1,odhat1,qdhat);
710  iCartCtrl->askForPose(qdhat,xd2,od2,xdhat2,odhat2,qdhat);
711 
712  double e_x1=norm(xd1-xdhat1);
713  double e_o1=norm(od1-odhat1);
714  printf("testing x=(%s); o=(%s) => xhat=(%s); ohat=(%s) ... |e_x|=%g; |e_o|=%g\n",
715  xd1.toString(3,3).c_str(),od1.toString(3,3).c_str(),
716  xdhat1.toString(3,3).c_str(),odhat1.toString(3,3).c_str(),
717  e_x1,e_o1);
718 
719  double e_x2=norm(xd2-xdhat2);
720  double e_o2=norm(od2-odhat2);
721  printf("testing x=(%s); o=(%s) => xhat=(%s); ohat=(%s) ... |e_x|=%g; |e_o|=%g\n",
722  xd2.toString(3,3).c_str(),od2.toString(3,3).c_str(),
723  xdhat2.toString(3,3).c_str(),odhat2.toString(3,3).c_str(),
724  e_x2,e_o2);
725 
726  double nearness_penalty=((norm(xdhat1)<0.15)||(norm(xdhat2)<0.15)?10.0:0.0);
727  printf("nearness penalty=%g\n",nearness_penalty);
728  res=e_x1+e_o1+e_x2+e_o2+nearness_penalty;
729  printf("final quality=%g\n",res);
730  }
731  // execute the movements
732  else
733  {
734  Vector offs(3,0.0); offs[2]=0.05;
735  if (!interrupting)
736  {
737  Vector x=xd1+offs;
738 
739  printf("moving to: x=(%s); o=(%s)\n",x.toString(3,3).c_str(),od1.toString(3,3).c_str());
740  iCartCtrl->goToPoseSync(x,od1,2.0);
741  iCartCtrl->waitMotionDone(0.1,5.0);
742  }
743 
744  if (!interrupting)
745  {
746  printf("moving to: x=(%s); o=(%s)\n",xd1.toString(3,3).c_str(),od1.toString(3,3).c_str());
747  iCartCtrl->goToPoseSync(xd1,od1,1.5);
748  iCartCtrl->waitMotionDone(0.1,5.0);
749  }
750 
751  if (!interrupting)
752  {
753  printf("moving to: x=(%s); o=(%s)\n",xd2.toString(3,3).c_str(),od2.toString(3,3).c_str());
754  iCartCtrl->goToPoseSync(xd2,od2,3.5);
755  iCartCtrl->waitMotionDone(0.1,5.0);
756  }
757  }
758 
759  iCartCtrl->restoreContext(context);
760  iCartCtrl->deleteContext(context);
761 
762  return res;
763  }
764 
765  /************************************************************************/
766  void shakeHand()
767  {
768  IEncoders *ienc;
769  IVelocityControl *ivel;
770 
771  if (handUsed=="left")
772  {
773  driverHL.view(ienc);
774  driverHL.view(ivel);
775  }
776  else
777  {
778  driverHR.view(ienc);
779  driverHR.view(ivel);
780  }
781 
782  double pos;
783  ienc->getEncoder(shake_joint,&pos);
784 
785  double e=flip_hand-pos;
786  if ((flip_hand>0.0) && (e<0.0) ||
787  (flip_hand<0.0) && (e>0.0))
788  {
789  flip_hand=-flip_hand;
790  e=flip_hand-pos;
791  }
792 
793  ivel->velocityMove(shake_joint,120.0*sign(e));
794  }
795 
796  /************************************************************************/
797  void stopHand(const string &hand)
798  {
799  IVelocityControl *ivel;
800  if (hand=="left")
801  driverHL.view(ivel);
802  else
803  driverHR.view(ivel);
804 
805  ivel->stop(4);
806  }
807 
808  /************************************************************************/
809  void moveTool(const string &arm, const string &eye, const Vector &xd, const Vector &od,
810  const Vector &xOffset, const int maxItems)
811  {
812  iGaze->restoreContext(0);
813 
814  if (!interrupting)
815  {
816  iGaze->setTrackingMode(true);
817  iGaze->lookAtFixationPoint(xd+xOffset);
818  iCartCtrl->goToPoseSync(xd,od,1.0);
819  iCartCtrl->waitMotionDone(0.1);
820  }
821 
822  iGaze->setSaccadesMode(false);
823  iGaze->setNeckTrajTime(2.5);
824  iGaze->setEyesTrajTime(1.5);
825 
826  // put the shaking joint in velocity mode
827  IControlMode *imode;
828  if (arm=="left")
829  driverHL.view(imode);
830  else
831  driverHR.view(imode);
832  imode->setControlMode(shake_joint,VOCAB_CM_VELOCITY);
833  handUsed=arm; // this triggers the hand shaking
834 
835  // gaze robustly at the tool tip
836  Vector pxCum(2,0.0);
837  int cnt=0; bool done=false;
838  double t0=Time::now();
839  while (!interrupting && !done)
840  {
841  double t1=Time::now();
842  if (Bottle *target=visionPort.read(false))
843  {
844  if (target->size()>=2)
845  {
846  Vector px(2);
847  px[0]=target->get(0).asDouble();
848  px[1]=target->get(1).asDouble()+50.0;
849  iGaze->lookAtMonoPixel(eye=="left"?0:1,px);
850 
851  pxCum+=px;
852  cnt++;
853  }
854  }
855 
856  if (t1-t0>=3.0)
857  {
858  if (cnt>20)
859  done=fabs(pxCum[1]/cnt-120)<30.0;
860 
861  pxCum=0.0;
862  cnt=0;
863  t0=t1;
864  }
865 
866  Time::delay(0.02);
867  }
868 
869  // gather sufficient information
870  Bottle command,reply;
871  command.addVocab(Vocab::encode("enable"));
872  finderPort.write(command,reply);
873 
874  command.clear();
875  command.addVocab(Vocab::encode("num"));
876  finderPort.write(command,reply);
877  int curItems=reply.get(1).asInt();
878 
879  int nItems=0;
880  while (!interrupting && (nItems<curItems+maxItems))
881  {
882  finderPort.write(command,reply);
883  nItems=reply.get(1).asInt();
884 
885  if (Bottle *target=visionPort.read(false))
886  {
887  if (target->size()>=2)
888  {
889  Vector px(2);
890  px[0]=target->get(0).asDouble();
891  px[1]=target->get(1).asDouble()+50.0;
892  iGaze->lookAtMonoPixel(eye=="left"?0:1,px);
893  }
894  }
895 
896  Time::delay(0.1);
897  }
898 
899  command.clear();
900  command.addVocab(Vocab::encode("disable"));
901  finderPort.write(command,reply);
902 
903  handUsed="null";
904  stopHand(arm);
905  }
906 
907  /************************************************************************/
908  bool findToolTip(const string &arm, const string &eye, Bottle &reply)
909  {
910  if (arm=="left")
911  iCartCtrl=iCartCtrlL;
912  else if (arm=="right")
913  iCartCtrl=iCartCtrlR;
914  else
915  return false;
916 
917  int context_arm,context_gaze;
918  iCartCtrl->storeContext(&context_arm);
919  iGaze->storeContext(&context_gaze);
920 
921  Vector dof;
922  iCartCtrl->getDOF(dof);
923  dof=1.0; dof[0]=dof[1]=0.0;
924  iCartCtrl->setDOF(dof,dof);
925 
926  Bottle command;
927  command.addVocab(Vocab::encode("clear"));
928  finderPort.write(command,reply);
929 
930  // solving
931  command.clear();
932  command.addVocab(Vocab::encode("select"));
933  command.addString(arm);
934  command.addString(eye);
935  finderPort.write(command,reply);
936 
937  Matrix R(4,4);
938  R(0,0)=-1.0;
939  R(2,1)=-1.0;
940  R(1,2)=-1.0;
941  R(3,3)=+1.0;
942  Vector r(4,0.0); r[0]=-1.0;
943  Vector xd(3,0.0),od;
944  Vector offset(3,0.0); offset[2]=0.1;
945 
946  // point 1
947  r[3]=0.0;
948  od=dcm2axis(axis2dcm(r)*R);
949  xd[0]=-0.35;
950  shake_joint=4;
951  moveTool(arm,eye,xd,od,offset,25);
952 
953  // point 2
954  r[3]=CTRL_DEG2RAD*(arm=="left"?30.0:-30.0);
955  od=dcm2axis(axis2dcm(r)*R);
956  xd[1]=(arm=="left")?-0.15:0.15;
957  offset[1]=(arm=="left")?0.1:-0.1;
958  moveTool(arm,eye,xd,od,offset,25);
959 
960  // point 3
961  r[3]=CTRL_DEG2RAD*(arm=="left"?20.0:-20.0);
962  od=dcm2axis(axis2dcm(r)*R);
963  xd[2]=0.15;
964  offset[1]=(arm=="left")?0.2:-0.2;
965  offset[2]=0.1;
966  moveTool(arm,eye,xd,od,offset,25);
967 
968  // point 4
969  r[3]=CTRL_DEG2RAD*(arm=="left"?10.0:-10.0);
970  od=dcm2axis(axis2dcm(r)*R);
971  xd[0]=-0.3;
972  xd[1]=(arm=="left")?-0.05:0.05;
973  xd[2]=-0.05;
974  moveTool(arm,eye,xd,od,offset,25);
975 
976  // point 5
977  r[3]=CTRL_DEG2RAD*(arm=="left"?45.0:-45.0);
978  od=dcm2axis(axis2dcm(r)*R);
979  xd[0]=-0.35;
980  xd[1]=(arm=="left")?-0.05:0.05;
981  xd[2]=0.1;
982  offset[1]=(arm=="left")?0.1:-0.1;
983  moveTool(arm,eye,xd,od,offset,25);
984 
985  // point 6
986  xd[0]=-0.35;
987  xd[1]=(arm=="left")?-0.1:0.1;
988  xd[2]=0.0;
989  Vector r1(4,0.0); r1[2]=(arm=="left")?-1.0:1.0; r1[3]=CTRL_DEG2RAD*45.0;
990  Vector r2(4,0.0); r2[0]=(arm=="left")?1.0:-1.0; r2[3]=CTRL_DEG2RAD*45.0;
991  od=dcm2axis(axis2dcm(r2)*axis2dcm(r1)*R);
992  offset[0]=0;
993  offset[1]=(arm=="left")?-0.05:0.05;
994  offset[2]=0.1;
995  shake_joint=6;
996  moveTool(arm,eye,xd,od,offset,50);
997 
998  // solving
999  command.clear();
1000  command.addVocab(Vocab::encode("find"));
1001  finderPort.write(command,reply);
1002 
1003  iCartCtrl->restoreContext(context_arm);
1004  iCartCtrl->deleteContext(context_arm);
1005 
1006  iGaze->restoreContext(context_gaze);
1007  iGaze->deleteContext(context_gaze);
1008 
1009  return true;
1010  }
1011 
1012 public:
1013  /************************************************************************/
1014  bool configure(ResourceFinder &rf)
1015  {
1016  string name=rf.check("name",Value("karmaMotor")).asString();
1017  string robot=rf.check("robot",Value("icub")).asString();
1018  elbow_set=rf.check("elbow_set");
1019  if (elbow_set)
1020  {
1021  if (Bottle *pB=rf.find("elbow_set").asList())
1022  {
1023  elbow_height=pB->get(0).asDouble();
1024  elbow_weight=pB->get(1).asDouble();
1025  }
1026  else
1027  {
1028  elbow_height=0.4;
1029  elbow_weight=30.0;
1030  }
1031  }
1032 
1033  Property optionG("(device gazecontrollerclient)");
1034  optionG.put("remote","/iKinGazeCtrl");
1035  optionG.put("local","/"+name+"/gaze_ctrl");
1036 
1037  Property optionL("(device cartesiancontrollerclient)");
1038  optionL.put("remote","/"+robot+"/cartesianController/left_arm");
1039  optionL.put("local","/"+name+"/cart_ctrl/left_arm");
1040 
1041  Property optionR("(device cartesiancontrollerclient)");
1042  optionR.put("remote","/"+robot+"/cartesianController/right_arm");
1043  optionR.put("local","/"+name+"/cart_ctrl/right_arm");
1044 
1045  Property optionHL("(device remote_controlboard)");
1046  optionHL.put("remote","/"+robot+"/left_arm");
1047  optionHL.put("local","/"+name+"/hand_ctrl/left_arm");
1048 
1049  Property optionHR("(device remote_controlboard)");
1050  optionHR.put("remote","/"+robot+"/right_arm");
1051  optionHR.put("local","/"+name+"/hand_ctrl/right_arm");
1052 
1053  if (!driverG.open(optionG))
1054  return false;
1055 
1056  if (!driverL.open(optionL))
1057  {
1058  driverG.close();
1059  return false;
1060  }
1061 
1062  if (!driverR.open(optionR))
1063  {
1064  driverG.close();
1065  driverL.close();
1066  return false;
1067  }
1068 
1069  if (!driverHL.open(optionHL))
1070  {
1071  driverG.close();
1072  driverL.close();
1073  driverR.close();
1074  return false;
1075  }
1076 
1077  if (!driverHR.open(optionHR))
1078  {
1079  driverG.close();
1080  driverL.close();
1081  driverR.close();
1082  driverHL.close();
1083  return false;
1084  }
1085 
1086  driverG.view(iGaze);
1087  driverL.view(iCartCtrlL);
1088  driverR.view(iCartCtrlR);
1089 
1090  visionPort.open("/"+name+"/vision:i");
1091  finderPort.open("/"+name+"/finder:rpc");
1092  rpcPort.open("/"+name+"/rpc");
1093  stopPort.open("/"+name+"/stop:i");
1094  attach(rpcPort);
1095  stopPort.setReader(*this);
1096 
1097  interrupting=false;
1098  handUsed="null";
1099  flip_hand=6.0;
1100 
1101  pushHand="selectable";
1102  toolFrame=eye(4,4);
1103 
1104  return true;
1105  }
1106 
1107  /************************************************************************/
1108  bool interruptModule()
1109  {
1110  interrupting=true;
1111 
1112  iGaze->stopControl();
1113  iCartCtrlL->stopControl();
1114  iCartCtrlR->stopControl();
1115 
1116  if (handUsed!="null")
1117  {
1118  stopHand("left");
1119  stopHand("right");
1120  }
1121 
1122  return true;
1123  }
1124 
1125  /************************************************************************/
1126  bool close()
1127  {
1128  visionPort.close();
1129  finderPort.close();
1130  rpcPort.close();
1131  stopPort.close(); // close prior to shutting down motor-interfaces
1132 
1133  driverG.close();
1134  driverL.close();
1135  driverR.close();
1136  driverHL.close();
1137  driverHR.close();
1138  return true;
1139  }
1140 
1141  /************************************************************************/
1142  double getPeriod()
1143  {
1144  return 0.02;
1145  }
1146 
1147  /************************************************************************/
1148  bool updateModule()
1149  {
1150  if (!interrupting && (handUsed!="null"))
1151  shakeHand();
1152 
1153  return true;
1154  }
1155 };
1156 
1157 
1158 /************************************************************************/
1159 int main(int argc, char *argv[])
1160 {
1161  Network yarp;
1162  if (!yarp.checkNetwork())
1163  {
1164  printf("YARP server not available!\n");
1165  return 1;
1166  }
1167 
1168  ResourceFinder rf;
1169  rf.configure(argc,argv);
1170 
1171  KarmaMotor karmaMotor;
1172  return karmaMotor.runModule(rf);
1173 }
1174 
1175 
1176 
Generated by doxygen 1.9.1