icub-basic-demos
main.cpp
1 /*
2  * Copyright (C) 2010 RobotCub Consortium, European Commission FP6 Project IST-004370
3  * Author: Ugo Pattacini
4  * email: ugo.pattacini@iit.it
5  * website: www.robotcub.org
6  * Permission is granted to copy, distribute, and/or modify this program
7  * under the terms of the GNU General Public License, version 2 or any
8  * later version published by the Free Software Foundation.
9  *
10  * A copy of the license can be found at
11  * http://www.robotcub.org/icub/license/gpl.txt
12  *
13  * This program is distributed in the hope that it will be useful, but
14  * WITHOUT ANY WARRANTY; without even the implied warranty of
15  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General
16  * Public License for more details
17 */
18 
244 #include <string>
245 #include <cmath>
246 #include <vector>
247 #include <algorithm>
248 
249 #include <yarp/os/all.h>
250 #include <yarp/dev/all.h>
251 #include <yarp/sig/Vector.h>
252 #include <yarp/math/Math.h>
253 #include <yarp/math/Rand.h>
254 
255 #include <iCub/ctrl/neuralNetworks.h>
256 #include <iCub/iKin/iKinFwd.h>
257 
258 #define DEFAULT_THR_PER 20
259 
260 #define NOARM 0
261 #define LEFTARM 1
262 #define RIGHTARM 2
263 #define USEDARM 3
264 
265 #define OPENHAND 0
266 #define CLOSEHAND 1
267 
268 #define FACE_HAPPY ("hap")
269 #define FACE_SAD ("sad")
270 #define FACE_ANGRY ("ang")
271 #define FACE_SHY ("shy")
272 #define FACE_EVIL ("evi")
273 #define FACE_CUNNING ("cun")
274 #define FACE_SURPRISED ("sur")
275 
276 #define STATE_IDLE 0
277 #define STATE_REACH 1
278 #define STATE_CHECKMOTIONDONE 2
279 #define STATE_RELEASE 3
280 #define STATE_WAIT 4
281 
282 using namespace std;
283 using namespace yarp::os;
284 using namespace yarp::sig;
285 using namespace yarp::dev;
286 using namespace yarp::math;
287 using namespace iCub::ctrl;
288 using namespace iCub::iKin;
289 
290 
291 class Predictor
292 {
293 protected:
294  ff2LayNN_tansig_purelin net;
295 
296 public:
297  bool configure(Property &options)
298  {
299  if (net.configure(options))
300  {
301  net.printStructure();
302  return true;
303  }
304  else
305  return false;
306  }
307 
308  Vector predict(const Vector &head, Bottle *imdLeft, Bottle *imdRight)
309  {
310  Bottle *firstBlobLeft=imdLeft->get(0).asList();
311  Bottle *firstBlobRight=imdRight->get(0).asList();
312 
313  Vector in(7);
314  in[0]=head[3]; // tilt
315  in[1]=head[4]; // pan
316  in[2]=head[5]; // ver
317  in[3]=firstBlobLeft->get(0).asFloat64(); // ul
318  in[4]=firstBlobLeft->get(1).asFloat64(); // vl
319  in[5]=firstBlobRight->get(0).asFloat64(); // ur
320  in[6]=firstBlobRight->get(1).asFloat64(); // vr
321 
322  return net.predict(in);
323  }
324 };
325 
326 
327 class managerThread : public PeriodicThread
328 {
329 protected:
330  ResourceFinder &rf;
331 
332  string name;
333  string robot;
334  string eyeUsed;
335 
336  std::vector<string> speech_grasp;
337  std::vector<string> speech_reach;
338  std::vector<string> speech_idle;
339 
340  bool useSpeech;
341  bool useLeftArm;
342  bool useRightArm;
343  bool useTorso;
344  int armSel;
345  bool simulation;
346  bool go;
347 
348  PolyDriver *drvTorso, *drvHead, *drvLeftArm, *drvRightArm;
349  PolyDriver *drvCartLeftArm, *drvCartRightArm;
350  PolyDriver *drvGazeCtrl;
351 
352  IEncoders *encTorso;
353  IEncoders *encHead;
354  IControlMode *modeTorso;
355  IPositionControl *posTorso;
356  IEncoders *encArm;
357  IControlMode *modeArm;
358  IPositionControl *posArm;
359  ICartesianControl *cartArm;
360  IGazeControl *gazeCtrl;
361 
362  BufferedPort<Bottle> inportTrackTarget;
363  BufferedPort<Bottle> inportIMDTargetLeft;
364  BufferedPort<Bottle> inportIMDTargetRight;
365  Port outportGui;
366  Port outportCmdFace;
367  Port outportSpeech;
368 
369  RpcClient breatherHrpc;
370  RpcClient breatherLArpc;
371  RpcClient breatherRArpc;
372  RpcClient blinkerrpc;
373  RpcClient lookSkinrpc;
374  BufferedPort<Bottle> gazeboMoverPort;
375 
376  Vector leftArmReachOffs;
377  Vector leftArmGraspOffs;
378  Vector leftArmHandOrien;
379  Vector leftArmJointsStiffness;
380  Vector leftArmJointsDamping;
381 
382  Vector rightArmReachOffs;
383  Vector rightArmGraspOffs;
384  Vector rightArmHandOrien;
385  Vector rightArmJointsStiffness;
386  Vector rightArmJointsDamping;
387 
388  Vector *armReachOffs;
389  Vector *armGraspOffs;
390  Vector *armHandOrien;
391 
392  Vector homePoss, homeVels;
393 
394  Predictor pred;
395  bool useNetwork;
396  bool wentHome;
397  bool leftGraspEnable;
398  bool rightGraspEnable;
399  bool leftArmImpVelMode;
400  bool rightArmImpVelMode;
401 
402  double trajTime;
403  double reachTol;
404  double idleTimer, idleTmo;
405  double hystThres;
406  double sphereRadius, sphereTmo;
407  double releaseTmo;
408 
409  double latchTimer;
410  Vector sphereCenter;
411 
412  Vector openHandPoss, closeHandPoss;
413  Vector handVels;
414 
415  Vector targetPos;
416  Vector torso;
417  Vector head;
418 
419  Matrix R,Rx,Ry,Rz;
420 
421  int state;
422  bool state_breathers;
423  int startup_context_id_left;
424  int startup_context_id_right;
425  int startup_context_id_gaze;
426 
427  void breathersHandler(const bool sw)
428  {
429  Bottle msg,reply;
430  msg.addString(sw?"start":"stop");
431 
432  if (breatherHrpc.getOutputCount()>0)
433  {
434  breatherHrpc.write(msg);
435  }
436 
437  if (breatherLArpc.getOutputCount()>0)
438  {
439  breatherLArpc.write(msg);
440  }
441 
442  if (breatherRArpc.getOutputCount()>0)
443  {
444  breatherRArpc.write(msg);
445  }
446 
447  if (blinkerrpc.getOutputCount()>0)
448  {
449  blinkerrpc.write(msg);
450  }
451 
452  if (lookSkinrpc.getOutputCount()>0)
453  {
454  lookSkinrpc.write(msg);
455  }
456 
457  state_breathers = !sw;
458  }
459 
460  void sendSpeak(const string &txt)
461  {
462  if (outportSpeech.getOutputCount()>0)
463  {
464  Bottle msg,reply;
465  msg.addString(txt);
466  outportSpeech.write(msg);
467  }
468  }
469 
470  void getTorsoOptions(Bottle &b, const char *type, const int i, Vector &sw, Matrix &lim)
471  {
472  if (b.check(type))
473  {
474  Bottle &grp=b.findGroup(type);
475  sw[i]=grp.get(1).asString()=="on"?1.0:0.0;
476 
477  if (grp.check("min","Getting minimum value"))
478  {
479  lim(i,0)=1.0;
480  lim(i,1)=grp.find("min").asFloat64();
481  }
482 
483  if (grp.check("max","Getting maximum value"))
484  {
485  lim(i,2)=1.0;
486  lim(i,3)=grp.find("max").asFloat64();
487  }
488  }
489  }
490 
491  void getArmOptions(Bottle &b, bool &graspEnable, Vector &reachOffs,
492  Vector &graspOffs, Vector &orien, bool &impVelMode,
493  Vector &impStiff, Vector &impDamp)
494  {
495  graspEnable=b.check("grasp_enable",Value("on"),"Getting arm grasp mode").asString()=="on"?true:false;
496 
497  if (b.check("reach_offset","Getting reaching offset"))
498  {
499  Bottle &grp=b.findGroup("reach_offset");
500  int sz=grp.size()-1;
501  int len=sz>3?3:sz;
502 
503  for (int i=0; i<len; i++)
504  reachOffs[i]=grp.get(1+i).asFloat64();
505  }
506 
507  if (b.check("grasp_offset","Getting grasping offset"))
508  {
509  Bottle &grp=b.findGroup("grasp_offset");
510  int sz=grp.size()-1;
511  int len=sz>3?3:sz;
512 
513  for (int i=0; i<len; i++)
514  graspOffs[i]=grp.get(1+i).asFloat64();
515  }
516 
517  if (b.check("hand_orientation","Getting hand orientation"))
518  {
519  Bottle &grp=b.findGroup("hand_orientation");
520  int sz=grp.size()-1;
521  int len=sz>4?4:sz;
522 
523  for (int i=0; i<len; i++)
524  orien[i]=grp.get(1+i).asFloat64();
525  }
526 
527  impVelMode=b.check("impedance_velocity_mode",Value("off"),"Getting arm impedance-velocity-mode").asString()=="on"?true:false;
528 
529  if (b.check("impedance_stiffness","Getting joints stiffness"))
530  {
531  Bottle &grp=b.findGroup("impedance_stiffness");
532  size_t sz=grp.size()-1;
533  size_t len=sz>impStiff.length()?impStiff.length():sz;
534 
535  for (size_t i=0; i<len; i++)
536  impStiff[i]=grp.get(1+i).asFloat64();
537  }
538 
539  if (b.check("impedance_damping","Getting joints damping"))
540  {
541  Bottle &grp=b.findGroup("impedance_damping");
542  size_t sz=grp.size()-1;
543  size_t len=sz>impDamp.length()?impDamp.length():sz;
544 
545  for (size_t i=0; i<len; i++)
546  impDamp[i]=grp.get(1+i).asFloat64();
547  }
548  }
549 
550  bool getHomeOptions(Bottle &b, Vector &poss, Vector &vels)
551  {
552  bool ret = true;
553  if (b.check("poss","Getting home poss"))
554  {
555  Bottle &grp=b.findGroup("poss");
556  int sz=grp.size()-1;
557  int len=sz>7?7:sz;
558 
559  for (int i=0; i<len; i++)
560  poss[i]=grp.get(1+i).asFloat64();
561  }
562  else
563  {
564  yError("Missing 'poss' parameter");
565  ret = false;
566  }
567 
568  if (b.check("vels","Getting home vels"))
569  {
570  Bottle &grp=b.findGroup("vels");
571  int sz=grp.size()-1;
572  int len=sz>7?7:sz;
573 
574  for (int i=0; i<len; i++)
575  vels[i]=grp.get(1+i).asFloat64();
576  }
577  else
578  {
579  yError("Missing 'vels' parameter");
580  ret = false;
581  }
582  return ret;
583  }
584 
585  bool getGraspOptions(Bottle &b, Vector &openPoss, Vector &closePoss, Vector &vels)
586  {
587  bool ret = true;
588  if (b.check("open_hand","Getting openHand poss"))
589  {
590  Bottle &grp=b.findGroup("open_hand");
591  int sz=grp.size()-1;
592  int len=sz>9?9:sz;
593 
594  for (int i=0; i<len; i++)
595  openPoss[i]=grp.get(1+i).asFloat64();
596  }
597  else
598  {
599  yError("Missing 'open_hand' parameter");
600  ret = false;
601  }
602 
603  if (b.check("close_hand","Getting closeHand poss"))
604  {
605  Bottle &grp=b.findGroup("close_hand");
606  int sz=grp.size()-1;
607  int len=sz>9?9:sz;
608 
609  for (int i=0; i<len; i++)
610  closePoss[i]=grp.get(1+i).asFloat64();
611  }
612  else
613  {
614  yError("Missing 'close_hand' parameter");
615  ret = false;
616  }
617 
618  if (b.check("vels_hand","Getting hand vels"))
619  {
620  Bottle &grp=b.findGroup("vels_hand");
621  int sz=grp.size()-1;
622  int len=sz>9?9:sz;
623 
624  for (int i=0; i<len; i++)
625  vels[i]=grp.get(1+i).asFloat64();
626  }
627  else
628  {
629  yError("Missing 'vels_hand' parameter");
630  ret = false;
631  }
632  return ret;
633  }
634 
635  void getSpeechOptions(Bottle &b, std::vector<string> &grasp,
636  std::vector<string> &reach, std::vector<string> &idle)
637  {
638  Bottle &bSpeechGrasp=b.findGroup("speech_grasp");
639  for (int i=1; i<bSpeechGrasp.size(); i++)
640  {
641  std::string str = bSpeechGrasp.get(i).asList()->toString();
642  str.erase(std::remove(str.begin(), str.end(), '\"'), str.end());
643  grasp.push_back(str);
644  }
645 
646  Bottle &bSpeechReach=b.findGroup("speech_reach");
647  for (int i=1; i<bSpeechReach.size(); i++)
648  {
649  std::string str = bSpeechReach.get(i).asList()->toString();
650  str.erase(std::remove(str.begin(), str.end(), '\"'), str.end());
651  reach.push_back(str);
652  }
653 
654  Bottle &bSpeechIdle=b.findGroup("speech_idle");
655  for (int i=1; i<bSpeechIdle.size(); i++)
656  {
657  std::string str = bSpeechIdle.get(i).asList()->toString();
658  str.erase(std::remove(str.begin(), str.end(), '\"'), str.end());
659  idle.push_back(str);
660  }
661  }
662 
663  void initCartesianCtrl(const Vector &sw, const Matrix &lim, const int sel=USEDARM)
664  {
665  ICartesianControl *icart=cartArm;
666  Vector dof;
667  string type;
668 
669  if (sel==LEFTARM)
670  {
671  if (useLeftArm)
672  {
673  drvCartLeftArm->view(icart);
674  icart->storeContext(&startup_context_id_left);
675  icart->restoreContext(0);
676  }
677  else
678  return;
679 
680  type="left_arm";
681  }
682  else if (sel==RIGHTARM)
683  {
684  if (useRightArm)
685  {
686  drvCartRightArm->view(icart);
687  icart->storeContext(&startup_context_id_right);
688  icart->restoreContext(0);
689  }
690  else
691  return;
692 
693  type="right_arm";
694  }
695  else if (armSel!=NOARM)
696  type=armSel==LEFTARM?"left_arm":"right_arm";
697  else
698  return;
699 
700  yInfo("*** Initializing %s controller ...",type.c_str());
701 
702  icart->setTrackingMode(false);
703  icart->setTrajTime(trajTime);
704  icart->setInTargetTol(reachTol);
705  icart->getDOF(dof);
706 
707  Bottle info;
708  icart->getInfo(info);
709  auto hwver=iKinLimbVersion(info.find("arm_version").asString());
710 
711  if (useTorso)
712  {
713  Vector sw_ = sw;
714  Matrix lim_=lim;
715  if (hwver>=iKinLimbVersion("3.0"))
716  {
717  sw_[0]=sw[1];
718  sw_[1]=sw[0];
719 
720  lim_.setSubrow(lim.getRow(1),0,0);
721  lim_.setSubrow(lim.getRow(0),1,0);
722  }
723 
724  for (size_t j=0; j<sw_.length(); j++)
725  {
726  dof[j]=sw_[j];
727  if ((sw_[j]!=0.0) && ((lim_(j,0)!=0.0) || (lim_(j,2)!=0.0)))
728  {
729  double min, max;
730  icart->getLimits(j,&min,&max);
731 
732  if (lim_(j,0)!=0.0)
733  min=lim_(j,1);
734 
735  if (lim_(j,2)!=0.0)
736  max=lim_(j,3);
737 
738  bool ok=icart->setLimits(j,min,max);
739  yInfo("jnt #%d in [%g, %g] deg => %s",(int)j,min,max,ok?"ok":"failed");
740  }
741  }
742  }
743  // there exist robots w/o torso, hence equipped w/ only 7 DOFs
744  else if (dof.size()>7)
745  {
746  dof[0]=dof[1]=dof[2]=0.0;
747  yInfo("Disabled torso joints");
748  }
749 
750  icart->setDOF(dof,dof);
751  yInfo("DOF=(%s)",dof.toString(0,1).c_str());
752  }
753 
754  void getSensorData()
755  {
756  bool newTarget=false;
757  if (useTorso)
758  if (encTorso->getEncoders(torso.data()))
759  R=rotx(torso[1])*roty(-torso[2])*rotz(-torso[0]);
760  encHead->getEncoders(head.data());
761 
762  if (useNetwork)
763  {
764  Bottle *imdTargetLeft=inportIMDTargetLeft.read(false);
765  Bottle *imdTargetRight=inportIMDTargetRight.read(false);
766 
767  if ((imdTargetLeft!=NULL) && (imdTargetRight!=NULL))
768  {
769  Vector x,o;
770  if (eyeUsed=="left")
771  gazeCtrl->getLeftEyePose(x,o);
772  else
773  gazeCtrl->getRightEyePose(x,o);
774 
775  Matrix T=axis2dcm(o);
776  T.setSubcol(x,0,3);
777 
778  Vector netout=pred.predict(head,imdTargetLeft,imdTargetRight);
779  netout.push_back(1.0);
780  targetPos=(T*netout).subVector(0,2);
781  newTarget=true;
782  }
783  }
784  else if (Bottle *targetPosNew=inportTrackTarget.read(false))
785  {
786  if (targetPosNew->size()>6)
787  {
788  if (targetPosNew->get(6).asFloat64()==1.0)
789  {
790  Vector fp(4);
791  fp[0]=targetPosNew->get(0).asFloat64();
792  fp[1]=targetPosNew->get(1).asFloat64();
793  fp[2]=targetPosNew->get(2).asFloat64();
794  fp[3]=1.0;
795 
796  if ((isnan(fp[0])==0) && (isnan(fp[1])==0) && (isnan(fp[2])==0))
797  {
798  Vector x,o;
799  if (eyeUsed=="left")
800  gazeCtrl->getLeftEyePose(x,o);
801  else
802  gazeCtrl->getRightEyePose(x,o);
803 
804  Matrix T=axis2dcm(o);
805  T.setSubcol(x,0,3);
806 
807  targetPos=T*fp;
808  targetPos.pop_back();
809  newTarget=true;
810  }
811  }
812  }
813  }
814 
815  if (newTarget)
816  {
817  idleTimer=Time::now();
818 
819  if (state==STATE_IDLE)
820  {
821  resetTargetBall();
822  breathersHandler(false);
823  yInfo("--- Got target => REACHING");
824 
825  wentHome=false;
826  state=STATE_REACH;
827  if(useSpeech) sendSpeak(speech_reach[(int)Rand::scalar(0,speech_reach.size()-1e-3)]);
828  }
829  }
830  else if (((state==STATE_IDLE) || (state==STATE_REACH)) &&
831  ((Time::now()-idleTimer)>idleTmo) && !wentHome && !simulation)
832  {
833  yInfo("--- Target timeout => IDLE");
834 
835  stopControl();
836  steerTorsoToHome();
837  steerHeadToHome();
838  steerArmToHome(LEFTARM);
839  steerArmToHome(RIGHTARM);
840 
841  wentHome=true;
842  deleteGuiTarget();
843  if(useSpeech) sendSpeak(speech_idle[(int)Rand::scalar(0,speech_idle.size()-1e-3)]);
844  state=STATE_IDLE;
845  }
846  }
847 
848  void doIdle()
849  {
850  if (state==STATE_IDLE)
851  {
852  if (state_breathers)
853  if (checkForHomePos())
854  breathersHandler(true);
855  }
856  }
857 
858  bool checkForHomePos()
859  {
860  IEncoders *iencsLA;
861  IEncoders *iencsRA;
862  if (useLeftArm) drvLeftArm->view(iencsLA);
863  if (useRightArm) drvRightArm->view(iencsRA);
864 
865  if (breatherHrpc.getOutputCount()>0)
866  {
867  bool done;
868  gazeCtrl->checkMotionDone(&done);
869  if (!done)
870  return false;
871  }
872 
873  int axes;
874  Vector encs;
875 
876  if (useLeftArm && breatherLArpc.getOutputCount()>0)
877  {
878  iencsLA->getAxes(&axes);
879  encs.resize(axes,0.0);
880  iencsLA->getEncoders(encs.data());
881  if (norm(encs.subVector(0,homePoss.length()-1)-homePoss)>4.0)
882  return false;
883  }
884 
885  if (useRightArm && breatherRArpc.getOutputCount()>0)
886  {
887  iencsRA->getAxes(&axes);
888  encs.resize(axes,0.0);
889  iencsRA->getEncoders(encs.data());
890  if (norm(encs.subVector(0,homePoss.length()-1)-homePoss)>4.0)
891  return false;
892  }
893 
894  return true;
895  }
896 
897  void commandHead()
898  {
899  if (state!=STATE_IDLE)
900  {
901  gazeCtrl->lookAtFixationPoint(targetPos);
902 
903  if (outportGui.getOutputCount()>0)
904  {
905  Bottle obj;
906  obj.addString("object");
907  obj.addString("ball");
908 
909  // size
910  obj.addFloat64(50.0);
911  obj.addFloat64(50.0);
912  obj.addFloat64(50.0);
913 
914  // positions
915  obj.addFloat64(1000.0*targetPos[0]);
916  obj.addFloat64(1000.0*targetPos[1]);
917  obj.addFloat64(1000.0*targetPos[2]);
918 
919  // orientation
920  obj.addFloat64(0.0);
921  obj.addFloat64(0.0);
922  obj.addFloat64(0.0);
923 
924  // color
925  obj.addInt32(255);
926  obj.addInt32(0);
927  obj.addInt32(0);
928 
929  // transparency
930  obj.addFloat64(1.0);
931 
932  outportGui.write(obj);
933  }
934  }
935  }
936 
937  void steerHeadToHome()
938  {
939  Vector homeHead(3);
940 
941  homeHead[0]=-1.0;
942  homeHead[1]=0.0;
943  homeHead[2]=0.3;
944 
945  yInfo("*** Homing head");
946 
947  gazeCtrl->lookAtFixationPoint(homeHead);
948  }
949 
950  void steerTorsoToHome()
951  {
952  if (!useTorso)
953  return;
954 
955  Vector homeTorso(3);
956  homeTorso.zero();
957 
958  Vector velTorso(3);
959  velTorso=10.0;
960 
961  yInfo("*** Homing torso");
962 
963  vector<int> modes(3,VOCAB_CM_POSITION);
964  modeTorso->setControlModes(modes.data());
965 
966  posTorso->setRefSpeeds(velTorso.data());
967  posTorso->positionMove(homeTorso.data());
968  }
969 
970  void checkTorsoHome(const double timeout=10.0)
971  {
972  if (!useTorso)
973  return;
974 
975  yInfo("*** Checking torso home position... ");
976 
977  bool done=false;
978  double t0=Time::now();
979  while (!done && (Time::now()-t0<timeout))
980  {
981  posTorso->checkMotionDone(&done);
982  Time::delay(0.1);
983  }
984 
985  yInfo("*** done");
986  }
987 
988  void steerArmToHome(const int sel=USEDARM)
989  {
990  IControlMode *imode=modeArm;
991  IPositionControl *ipos=posArm;
992  string type;
993 
994  if (sel==LEFTARM)
995  {
996  if (useLeftArm)
997  {
998  drvLeftArm->view(imode);
999  drvLeftArm->view(ipos);
1000  }
1001  else
1002  return;
1003 
1004  type="left_arm";
1005  }
1006  else if (sel==RIGHTARM)
1007  {
1008  if (useRightArm)
1009  {
1010  drvRightArm->view(imode);
1011  drvRightArm->view(ipos);
1012  }
1013  else
1014  return;
1015 
1016  type="right_arm";
1017  }
1018  else if (armSel!=NOARM)
1019  type=armSel==LEFTARM?"left_arm":"right_arm";
1020  else
1021  return;
1022 
1023  yInfo("*** Homing %s",type.c_str());
1024  for (size_t j=0; j<homeVels.length(); j++)
1025  imode->setControlMode(j,VOCAB_CM_POSITION);
1026 
1027  for (size_t j=0; j<homeVels.length(); j++)
1028  {
1029  ipos->setRefSpeed(j,homeVels[j]);
1030  ipos->positionMove(j,homePoss[j]);
1031  }
1032 
1033  openHand(sel);
1034  }
1035 
1036  void checkArmHome(const int sel=USEDARM, const double timeout=10.0)
1037  {
1038  IPositionControl *ipos=posArm;
1039  string type;
1040 
1041  if (sel==LEFTARM)
1042  {
1043  if (useLeftArm)
1044  drvLeftArm->view(ipos);
1045  else
1046  return;
1047 
1048  type="left_arm";
1049  }
1050  else if (sel==RIGHTARM)
1051  {
1052  if (useRightArm)
1053  drvRightArm->view(ipos);
1054  else
1055  return;
1056 
1057  type="right_arm";
1058  }
1059  else if (armSel!=NOARM)
1060  type=armSel==LEFTARM?"left_arm":"right_arm";
1061  else
1062  return;
1063 
1064  yInfo("*** Checking %s home position... ",type.c_str());
1065 
1066  bool done=false;
1067  double t0=Time::now();
1068  while (!done && (Time::now()-t0<timeout))
1069  {
1070  ipos->checkMotionDone(&done);
1071  Time::delay(0.1);
1072  }
1073 
1074  yInfo("*** done");
1075  }
1076 
1077  void stopArmJoints(const int sel=USEDARM)
1078  {
1079  IEncoders *ienc=encArm;
1080  IControlMode *imode=modeArm;
1081  IPositionControl *ipos=posArm;
1082  string type;
1083 
1084  if (sel==LEFTARM)
1085  {
1086  if (useLeftArm)
1087  {
1088  drvLeftArm->view(ienc);
1089  drvLeftArm->view(imode);
1090  drvLeftArm->view(ipos);
1091  }
1092  else
1093  return;
1094 
1095  type="left_arm";
1096  }
1097  else if (sel==RIGHTARM)
1098  {
1099  if (useRightArm)
1100  {
1101  drvRightArm->view(ienc);
1102  drvRightArm->view(imode);
1103  drvRightArm->view(ipos);
1104  }
1105  else
1106  return;
1107 
1108  type="right_arm";
1109  }
1110  else if (armSel!=NOARM)
1111  type=armSel==LEFTARM?"left_arm":"right_arm";
1112  else
1113  return;
1114 
1115  yInfo("*** Stopping %s joints",type.c_str());
1116  for (size_t j=0; j<homeVels.length(); j++)
1117  imode->setControlMode(j,VOCAB_CM_POSITION);
1118 
1119  for (size_t j=0; j<homeVels.length(); j++)
1120  {
1121  double fb;
1122  ienc->getEncoder(j,&fb);
1123  ipos->positionMove(j,fb);
1124  }
1125  }
1126 
1127  void moveHand(const int action, const int sel=USEDARM)
1128  {
1129  IControlMode *imode=modeArm;
1130  IPositionControl *ipos=posArm;
1131  Vector *poss=NULL;
1132  string actionStr, type;
1133 
1134  switch (action)
1135  {
1136  case OPENHAND:
1137  poss=&openHandPoss;
1138  actionStr="Opening";
1139  break;
1140 
1141  case CLOSEHAND:
1142  poss=&closeHandPoss;
1143  actionStr="Closing";
1144  break;
1145 
1146  default:
1147  return;
1148  }
1149 
1150  if (sel==LEFTARM)
1151  {
1152  drvLeftArm->view(imode);
1153  drvLeftArm->view(ipos);
1154  type="left_hand";
1155  }
1156  else if (sel==RIGHTARM)
1157  {
1158  drvRightArm->view(imode);
1159  drvRightArm->view(ipos);
1160  type="right_hand";
1161  }
1162  else
1163  type=armSel==LEFTARM?"left_hand":"right_hand";
1164 
1165  yInfo("*** %s %s",actionStr.c_str(),type.c_str());
1166  for (size_t j=0; j<handVels.length(); j++)
1167  imode->setControlMode(homeVels.length()+j,VOCAB_CM_POSITION);
1168 
1169  for (size_t j=0; j<handVels.length(); j++)
1170  {
1171  int k=homeVels.length()+j;
1172  ipos->setRefSpeed(k,handVels[j]);
1173  ipos->positionMove(k,(*poss)[j]);
1174  }
1175  }
1176 
1177  void openHand(const int sel=USEDARM)
1178  {
1179  moveHand(OPENHAND,sel);
1180  }
1181 
1182  void closeHand(const int sel=USEDARM)
1183  {
1184  moveHand(CLOSEHAND,sel);
1185  }
1186 
1187  void selectArm()
1188  {
1189  if (useLeftArm && useRightArm)
1190  {
1191  if (state==STATE_REACH)
1192  {
1193  // handle the hysteresis thresholds
1194  if ((armSel==LEFTARM) && (targetPos[1]>hystThres) ||
1195  (armSel==RIGHTARM) && (targetPos[1]<-hystThres))
1196  {
1197  yInfo("*** Change arm event triggered");
1198  state=STATE_CHECKMOTIONDONE;
1199  latchTimer=Time::now();
1200  }
1201  }
1202  else if (state==STATE_CHECKMOTIONDONE)
1203  {
1204  bool done;
1205  cartArm->checkMotionDone(&done);
1206  if (!done)
1207  {
1208  if (Time::now()-latchTimer>3.0*trajTime)
1209  {
1210  yInfo("--- Timeout elapsed => FORCE STOP and CHANGE ARM");
1211  done=true;
1212  }
1213  }
1214 
1215  if (done)
1216  {
1217  stopControl();
1218  steerArmToHome();
1219 
1220  // swap interfaces
1221  if (armSel==RIGHTARM)
1222  {
1223  armSel=LEFTARM;
1224 
1225  drvLeftArm->view(encArm);
1226  drvLeftArm->view(modeArm);
1227  drvLeftArm->view(posArm);
1228  drvCartLeftArm->view(cartArm);
1229  armReachOffs=&leftArmReachOffs;
1230  armGraspOffs=&leftArmGraspOffs;
1231  armHandOrien=&leftArmHandOrien;
1232  }
1233  else
1234  {
1235  armSel=RIGHTARM;
1236 
1237  drvRightArm->view(encArm);
1238  drvRightArm->view(modeArm);
1239  drvRightArm->view(posArm);
1240  drvCartRightArm->view(cartArm);
1241  armReachOffs=&rightArmReachOffs;
1242  armGraspOffs=&rightArmGraspOffs;
1243  armHandOrien=&rightArmHandOrien;
1244  }
1245 
1246  yInfo("*** Using %s",armSel==LEFTARM?"left_arm":"right_arm");
1247  stopArmJoints();
1248  state=STATE_REACH;
1249  }
1250  }
1251  }
1252  }
1253 
1254  void doReach()
1255  {
1256  if (useLeftArm || useRightArm)
1257  {
1258  if (state==STATE_REACH)
1259  {
1260  Vector x=R.transposed()*(targetPos+*armReachOffs);
1261  limitRange(x);
1262  x=R*x;
1263 
1264  cartArm->goToPoseSync(x,*armHandOrien);
1265  }
1266  }
1267  }
1268 
1269  void doGrasp()
1270  {
1271  if (useLeftArm || useRightArm)
1272  {
1273  if (state==STATE_REACH)
1274  {
1275  if (checkTargetForGrasp() && checkArmForGrasp())
1276  {
1277  Vector x=R.transposed()*(targetPos+*armGraspOffs);
1278  limitRange(x);
1279  x=R*x;
1280 
1281  yInfo("--- Hand in position AND Target still => GRASPING");
1282  yInfo("--- Target in %s",targetPos.toString().c_str());
1283  yInfo("*** Grasping x=%s",x.toString().c_str());
1284 
1285  //speak something
1286  if(useSpeech) sendSpeak(speech_grasp[(int)Rand::scalar(0,speech_grasp.size()-1e-3)]);
1287 
1288  cartArm->goToPoseSync(x,*armHandOrien);
1289  closeHand();
1290 
1291  latchTimer=Time::now();
1292  state=STATE_RELEASE;
1293  }
1294  }
1295  }
1296 
1297 
1298  }
1299 
1300  void doRelease()
1301  {
1302  if (useLeftArm || useRightArm)
1303  {
1304  if (state==STATE_RELEASE)
1305  {
1306  if ((Time::now()-latchTimer)>releaseTmo)
1307  {
1308  yInfo("--- Timeout elapsed => RELEASING");
1309 
1310  openHand();
1311 
1312  latchTimer=Time::now();
1313  state=STATE_WAIT;
1314  }
1315  }
1316  }
1317  }
1318 
1319  void doWait()
1320  {
1321  if (useLeftArm || useRightArm)
1322  {
1323  if (state==STATE_WAIT)
1324  {
1325  if ((Time::now()-latchTimer)>idleTmo)
1326  {
1327  yInfo("--- Timeout elapsed => IDLING");
1328  deleteGuiTarget();
1329  state=STATE_IDLE;
1330  }
1331  }
1332  }
1333  }
1334 
1335  void commandFace()
1336  {
1337  if (state==STATE_IDLE)
1338  setFace(state_breathers?FACE_SHY:FACE_HAPPY);
1339  else if (state==STATE_REACH)
1340  {
1341  if (useLeftArm || useRightArm)
1342  {
1343  if (checkArmForGrasp())
1344  setFace(FACE_EVIL);
1345  else
1346  setFace(FACE_ANGRY);
1347  }
1348  else
1349  setFace(FACE_EVIL);
1350  }
1351  else if (state==STATE_WAIT)
1352  setFace(FACE_HAPPY);
1353  }
1354 
1355  bool checkArmForGrasp()
1356  {
1357  Vector x,o;
1358  cartArm->getPose(x,o);
1359 
1360  // true if arm has reached the position
1361  if (norm(targetPos+*armReachOffs-x)<sphereRadius)
1362  return true;
1363  else
1364  return false;
1365  }
1366 
1367  bool checkTargetForGrasp()
1368  {
1369  const double t=Time::now();
1370 
1371  // false if target is considered to be still moving
1372  if (norm(targetPos-sphereCenter)>sphereRadius)
1373  {
1374  resetTargetBall();
1375  return false;
1376  }
1377  else if ((t-latchTimer<sphereTmo) || (t-idleTimer>1.0))
1378  return false;
1379  else
1380  return true;
1381  }
1382 
1383  void resetTargetBall()
1384  {
1385  latchTimer=Time::now();
1386  sphereCenter=targetPos;
1387  }
1388 
1389  void stopControl()
1390  {
1391  if (useLeftArm || useRightArm)
1392  {
1393  yInfo("stopping control");
1394  cartArm->stopControl();
1395  Time::delay(0.1);
1396  }
1397  }
1398 
1399  void setFace(const string &type)
1400  {
1401  Bottle in, out;
1402 
1403  out.addVocab32("set");
1404  out.addVocab32("mou");
1405  out.addVocab32(type);
1406  outportCmdFace.write(out,in);
1407 
1408  out.clear();
1409 
1410  out.addVocab32("set");
1411  out.addVocab32("leb");
1412  out.addVocab32(type);
1413  outportCmdFace.write(out,in);
1414 
1415  out.clear();
1416 
1417  out.addVocab32("set");
1418  out.addVocab32("reb");
1419  out.addVocab32(type);
1420  outportCmdFace.write(out,in);
1421  }
1422 
1423  void limitRange(Vector &x)
1424  {
1425  x[0]=x[0]>-0.1 ? -0.1 : x[0];
1426  }
1427 
1428  Matrix &rotx(const double theta)
1429  {
1430  double t=CTRL_DEG2RAD*theta;
1431  double c=cos(t);
1432  double s=sin(t);
1433 
1434  Rx(1,1)=Rx(2,2)=c;
1435  Rx(1,2)=-s;
1436  Rx(2,1)=s;
1437 
1438  return Rx;
1439  }
1440 
1441  Matrix &roty(const double theta)
1442  {
1443  double t=CTRL_DEG2RAD*theta;
1444  double c=cos(t);
1445  double s=sin(t);
1446 
1447  Ry(0,0)=Ry(2,2)=c;
1448  Ry(0,2)=s;
1449  Ry(2,0)=-s;
1450 
1451  return Ry;
1452  }
1453 
1454  Matrix &rotz(const double theta)
1455  {
1456  double t=CTRL_DEG2RAD*theta;
1457  double c=cos(t);
1458  double s=sin(t);
1459 
1460  Rz(0,0)=Rz(1,1)=c;
1461  Rz(0,1)=-s;
1462  Rz(1,0)=s;
1463 
1464  return Rz;
1465  }
1466 
1467  void deleteGuiTarget()
1468  {
1469  if (outportGui.getOutputCount()>0)
1470  {
1471  Bottle obj;
1472  obj.addString("delete");
1473  obj.addString("ball");
1474  outportGui.write(obj);
1475  }
1476  }
1477 
1478  void close()
1479  {
1480  delete drvTorso;
1481  delete drvHead;
1482  delete drvLeftArm;
1483  delete drvRightArm;
1484  delete drvCartLeftArm;
1485  delete drvCartRightArm;
1486  delete drvGazeCtrl;
1487 
1488  inportTrackTarget.interrupt();
1489  inportTrackTarget.close();
1490 
1491  inportIMDTargetLeft.interrupt();
1492  inportIMDTargetLeft.close();
1493 
1494  inportIMDTargetRight.interrupt();
1495  inportIMDTargetRight.close();
1496 
1497  setFace(FACE_HAPPY);
1498  outportCmdFace.interrupt();
1499  outportCmdFace.close();
1500 
1501  deleteGuiTarget();
1502  outportGui.interrupt();
1503  outportGui.close();
1504 
1505  outportSpeech.interrupt();
1506  outportSpeech.close();
1507 
1508  breatherHrpc.close();
1509  breatherLArpc.close();
1510  breatherRArpc.close();
1511  blinkerrpc.close();
1512  lookSkinrpc.close();
1513  if (simulation)
1514  {
1515  gazeboMoverPort.interrupt();
1516  gazeboMoverPort.close();
1517  }
1518  }
1519 
1520 public:
1521  managerThread(const string &_name, ResourceFinder &_rf) :
1522  PeriodicThread((double)DEFAULT_THR_PER/1000.0), name(_name), rf(_rf)
1523  {
1524  drvTorso=drvHead=drvLeftArm=drvRightArm=NULL;
1525  drvCartLeftArm=drvCartRightArm=NULL;
1526  drvGazeCtrl=NULL;
1527  }
1528 
1529  bool threadInit()
1530  {
1531  // general part
1532  Bottle &bGeneral=rf.findGroup("general");
1533  robot=bGeneral.check("robot",Value("icub"),"Getting robot name").asString();
1534  useLeftArm=bGeneral.check("left_arm",Value("on"),"Getting left arm use flag").asString()=="on"?true:false;
1535  useRightArm=bGeneral.check("right_arm",Value("on"),"Getting right arm use flag").asString()=="on"?true:false;
1536  useTorso=bGeneral.check("torso",Value("on"),"Getting torso use flag").asString()=="on"?true:false;
1537  useSpeech=bGeneral.check("speech",Value("on"),"Getting speech use flag").asString()=="on"?true:false;
1538  useNetwork=bGeneral.check("use_network",Value("off"),"Getting network enable").asString()=="on"?true:false;
1539  simulation=bGeneral.check("simulation",Value("off"),"Getting simulation enable").asString()=="on"?true:false;
1540  trajTime=bGeneral.check("traj_time",Value(2.0),"Getting trajectory time").asFloat64();
1541  reachTol=bGeneral.check("reach_tol",Value(0.01),"Getting reaching tolerance").asFloat64();
1542  eyeUsed=bGeneral.check("eye",Value("left"),"Getting the used eye").asString();
1543  idleTmo=bGeneral.check("idle_tmo",Value(1e10),"Getting idle timeout").asFloat64();
1544  setPeriod((double)bGeneral.check("thread_period",Value(DEFAULT_THR_PER),"Getting thread period [ms]").asInt32()/1000.0);
1545 
1546  if (!useTorso)
1547  {
1548  yWarning("Part \"torso\" is not employed!");
1549  }
1550 
1551  // torso part
1552  Bottle &bTorso=rf.findGroup("torso");
1553 
1554  Vector torsoSwitch(3); torsoSwitch.zero();
1555  Matrix torsoLimits(3,4); torsoLimits.zero();
1556 
1557  getTorsoOptions(bTorso,"pitch",0,torsoSwitch,torsoLimits);
1558  getTorsoOptions(bTorso,"roll",1,torsoSwitch,torsoLimits);
1559  getTorsoOptions(bTorso,"yaw",2,torsoSwitch,torsoLimits);
1560 
1561  // arm parts
1562  Bottle &bLeftArm=rf.findGroup("left_arm");
1563  Bottle &bRightArm=rf.findGroup("right_arm");
1564 
1565  leftArmReachOffs.resize(3,0.0);
1566  leftArmGraspOffs.resize(3,0.0);
1567  leftArmHandOrien.resize(4,0.0);
1568  leftArmJointsStiffness.resize(5,0.0);
1569  leftArmJointsDamping.resize(5,0.0);
1570  rightArmReachOffs.resize(3,0.0);
1571  rightArmGraspOffs.resize(3,0.0);
1572  rightArmHandOrien.resize(4,0.0);
1573  rightArmJointsStiffness.resize(5,0.0);
1574  rightArmJointsDamping.resize(5,0.0);
1575 
1576  getArmOptions(bLeftArm,leftGraspEnable,leftArmReachOffs,leftArmGraspOffs,
1577  leftArmHandOrien,leftArmImpVelMode,leftArmJointsStiffness,leftArmJointsDamping);
1578  getArmOptions(bRightArm,rightGraspEnable,rightArmReachOffs,rightArmGraspOffs,
1579  rightArmHandOrien,rightArmImpVelMode,rightArmJointsStiffness,rightArmJointsDamping);
1580 
1581  // home part
1582  Bottle &bHome=rf.findGroup("home_arm");
1583  homePoss.resize(7,0.0); homeVels.resize(7,0.0);
1584  if (!getHomeOptions(bHome, homePoss, homeVels)) { yError ("Error in parameters section 'home_arm'"); return false; }
1585 
1586  // arm_selection part
1587  Bottle &bArmSel=rf.findGroup("arm_selection");
1588  hystThres=bArmSel.check("hysteresis_thres",Value(0.0),"Getting hysteresis threshold").asFloat64();
1589 
1590  // grasp part
1591  Bottle &bGrasp=rf.findGroup("grasp");
1592  sphereRadius=bGrasp.check("sphere_radius",Value(0.0),"Getting sphere radius").asFloat64();
1593  sphereTmo=bGrasp.check("sphere_tmo",Value(0.0),"Getting sphere timeout").asFloat64();
1594  releaseTmo=bGrasp.check("release_tmo",Value(0.0),"Getting release timeout").asFloat64();
1595 
1596  openHandPoss.resize(9,0.0); closeHandPoss.resize(9,0.0);
1597  handVels.resize(9,0.0);
1598 
1599  if(!getGraspOptions(bGrasp, openHandPoss, closeHandPoss, handVels)) { yError ("Error in parameters section 'grasp'"); return false; }
1600 
1601  // init network
1602  if (useNetwork)
1603  {
1604  Property options;
1605  options.fromConfigFile(rf.findFile(bGeneral.check("network",Value("network.ini"),
1606  "Getting network data").asString()));
1607 
1608  if (!pred.configure(options))
1609  return false;
1610  }
1611 
1612  // open ports
1613  inportTrackTarget.open(name+"/trackTarget:i");
1614  inportIMDTargetLeft.open(name+"/imdTargetLeft:i");
1615  inportIMDTargetRight.open(name+"/imdTargetRight:i");
1616  outportCmdFace.open(name+"/cmdFace:rpc");
1617  outportGui.open(name+"/gui:o");
1618  outportSpeech.open(name+"/speech:o");
1619  breatherHrpc.open(name+"/breather/head:rpc");
1620  breatherLArpc.open(name+"/breather/left_arm:rpc");
1621  breatherRArpc.open(name+"/breather/right_arm:rpc");
1622  blinkerrpc.open(name+"/blinker:rpc");
1623  lookSkinrpc.open(name+"/lookSkin:rpc");
1624  if (simulation)
1625  {
1626  go=false;
1627  gazeboMoverPort.open(name+"/gazebo:o");
1628  if (!Network::connect(gazeboMoverPort.getName(),"/red-ball/mover:i"))
1629  {
1630  yError()<<"Unable to connect to the redball mover!";
1631  gazeboMoverPort.interrupt();
1632  gazeboMoverPort.close();
1633  return false;
1634  }
1635  }
1636  else
1637  {
1638  go=true;
1639  }
1640 
1641  string fwslash="/";
1642 
1643  // open remote_controlboard drivers
1644  Property optTorso("(device remote_controlboard)");
1645  Property optHead("(device remote_controlboard)");
1646  Property optLeftArm("(device remote_controlboard)");
1647  Property optRightArm("(device remote_controlboard)");
1648 
1649  optTorso.put("remote",fwslash+robot+"/torso");
1650  optTorso.put("local",name+"/torso");
1651 
1652  optHead.put("remote",fwslash+robot+"/head");
1653  optHead.put("local",name+"/head");
1654 
1655  optLeftArm.put("remote",fwslash+robot+"/left_arm");
1656  optLeftArm.put("local",name+"/left_arm");
1657 
1658  optRightArm.put("remote",fwslash+robot+"/right_arm");
1659  optRightArm.put("local",name+"/right_arm");
1660 
1661  if (useTorso)
1662  {
1663  drvTorso=new PolyDriver;
1664  if (!drvTorso->open(optTorso))
1665  {
1666  close();
1667  return false;
1668  }
1669  }
1670 
1671  drvHead=new PolyDriver;
1672  if (!drvHead->open(optHead))
1673  {
1674  close();
1675  return false;
1676  }
1677 
1678  if (useLeftArm)
1679  {
1680  drvLeftArm=new PolyDriver;
1681  if (!drvLeftArm->open(optLeftArm))
1682  {
1683  close();
1684  return false;
1685  }
1686  }
1687 
1688  if (useRightArm)
1689  {
1690  drvRightArm=new PolyDriver;
1691  if (!drvRightArm->open(optRightArm))
1692  {
1693  close();
1694  return false;
1695  }
1696  }
1697 
1698  // open cartesiancontrollerclient and gazecontrollerclient drivers
1699  Property optCartLeftArm("(device cartesiancontrollerclient)");
1700  Property optCartRightArm("(device cartesiancontrollerclient)");
1701  Property optGazeCtrl("(device gazecontrollerclient)");
1702 
1703  optCartLeftArm.put("remote",fwslash+robot+"/cartesianController/left_arm");
1704  optCartLeftArm.put("local",name+"/left_arm/cartesian");
1705 
1706  optCartRightArm.put("remote",fwslash+robot+"/cartesianController/right_arm");
1707  optCartRightArm.put("local",name+"/right_arm/cartesian");
1708 
1709  optGazeCtrl.put("remote","/iKinGazeCtrl");
1710  optGazeCtrl.put("local",name+"/gaze");
1711 
1712  if (useLeftArm)
1713  {
1714  drvCartLeftArm=new PolyDriver;
1715  if (!drvCartLeftArm->open(optCartLeftArm))
1716  {
1717  close();
1718  return false;
1719  }
1720 
1721  if (leftArmImpVelMode)
1722  {
1723  IInteractionMode *imode;
1724  IImpedanceControl *iimp;
1725 
1726  drvLeftArm->view(imode);
1727  drvLeftArm->view(iimp);
1728 
1729  int len=leftArmJointsStiffness.length()<leftArmJointsDamping.length()?
1730  leftArmJointsStiffness.length():leftArmJointsDamping.length();
1731 
1732  for (int j=0; j<len; j++)
1733  {
1734  iimp->setImpedance(j,leftArmJointsStiffness[j],leftArmJointsDamping[j]);
1735  imode->setInteractionMode(j,VOCAB_IM_COMPLIANT);
1736  }
1737  }
1738  }
1739 
1740  if (useRightArm)
1741  {
1742  drvCartRightArm=new PolyDriver;
1743  if (!drvCartRightArm->open(optCartRightArm))
1744  {
1745  close();
1746  return false;
1747  }
1748 
1749  if (rightArmImpVelMode)
1750  {
1751  IInteractionMode *imode;
1752  IImpedanceControl *iimp;
1753 
1754  drvRightArm->view(imode);
1755  drvRightArm->view(iimp);
1756 
1757  int len=rightArmJointsStiffness.length()<rightArmJointsDamping.length()?
1758  rightArmJointsStiffness.length():rightArmJointsDamping.length();
1759 
1760  for (int j=0; j<len; j++)
1761  {
1762  iimp->setImpedance(j,rightArmJointsStiffness[j],rightArmJointsDamping[j]);
1763  imode->setInteractionMode(j,VOCAB_IM_COMPLIANT);
1764  }
1765  }
1766  }
1767 
1768  drvGazeCtrl=new PolyDriver;
1769  if (!drvGazeCtrl->open(optGazeCtrl))
1770  {
1771  close();
1772  return false;
1773  }
1774 
1775  // open views
1776  if (useTorso)
1777  {
1778  drvTorso->view(modeTorso);
1779  drvTorso->view(encTorso);
1780  drvTorso->view(posTorso);
1781  }
1782  else
1783  {
1784  modeTorso=NULL;
1785  encTorso=NULL;
1786  posTorso=NULL;
1787  }
1788 
1789  drvHead->view(encHead);
1790  drvGazeCtrl->view(gazeCtrl);
1791 
1792  gazeCtrl->storeContext(&startup_context_id_gaze);
1793  gazeCtrl->restoreContext(0);
1794  gazeCtrl->blockNeckRoll(0.0);
1795  gazeCtrl->setSaccadesActivationAngle(20.0);
1796  gazeCtrl->setSaccadesInhibitionPeriod(1.0);
1797 
1798  if (useLeftArm)
1799  {
1800  drvLeftArm->view(encArm);
1801  drvLeftArm->view(modeArm);
1802  drvLeftArm->view(posArm);
1803  drvCartLeftArm->view(cartArm);
1804  armReachOffs=&leftArmReachOffs;
1805  armGraspOffs=&leftArmGraspOffs;
1806  armHandOrien=&leftArmHandOrien;
1807  armSel=LEFTARM;
1808  }
1809  else if (useRightArm)
1810  {
1811  drvRightArm->view(encArm);
1812  drvRightArm->view(modeArm);
1813  drvRightArm->view(posArm);
1814  drvCartRightArm->view(cartArm);
1815  armReachOffs=&rightArmReachOffs;
1816  armGraspOffs=&rightArmGraspOffs;
1817  armHandOrien=&rightArmHandOrien;
1818  armSel=RIGHTARM;
1819  }
1820  else
1821  {
1822  encArm=NULL;
1823  modeArm=NULL;
1824  posArm=NULL;
1825  cartArm=NULL;
1826  armReachOffs=NULL;
1827  armGraspOffs=NULL;
1828  armHandOrien=NULL;
1829  armSel=NOARM;
1830  }
1831 
1832  // init
1833  if (useTorso)
1834  {
1835  int torsoAxes;
1836  encTorso->getAxes(&torsoAxes);
1837  torso.resize(torsoAxes,0.0);
1838  }
1839 
1840  int headAxes;
1841  encHead->getAxes(&headAxes);
1842  head.resize(headAxes,0.0);
1843 
1844  targetPos.resize(3,0.0);
1845  R=Rx=Ry=Rz=eye(3,3);
1846 
1847  if (useLeftArm)
1848  {
1849  initCartesianCtrl(torsoSwitch,torsoLimits,LEFTARM);
1850  }
1851  if (useRightArm)
1852  {
1853  initCartesianCtrl(torsoSwitch,torsoLimits,RIGHTARM);
1854  }
1855 
1856  // steer the robot to the initial configuration
1857  stopControl();
1858  steerTorsoToHome();
1859  steerHeadToHome();
1860  steerArmToHome(LEFTARM);
1861  steerArmToHome(RIGHTARM);
1862 
1863  idleTimer=Time::now();
1864 
1865  wentHome=false;
1866  state=STATE_IDLE;
1867  state_breathers=true;
1868 
1869  // populate the speech strings
1870  if (useSpeech)
1871  {
1872  Rand::init();
1873  Bottle &bSpeech=rf.findGroup("speech");
1874  if (bSpeech.size()>0)
1875  {
1876  getSpeechOptions(bSpeech,speech_grasp,speech_reach,speech_idle);
1877  }
1878  else
1879  {
1880  yWarning("no speech group has been found even though speech flag option was true!");
1881  yWarning("setting speech flag option to false.");
1882  useSpeech = false;
1883  }
1884  }
1885 
1886  return true;
1887  }
1888 
1889  bool updateBall(const double &x, const double &y, const double &z)
1890  {
1891  if (simulation)
1892  {
1893  if (gazeboMoverPort.getOutputCount() > 0)
1894  {
1895  Bottle pose;
1896  pose.addFloat64(x);
1897  pose.addFloat64(y);
1898  pose.addFloat64(z);
1899  gazeboMoverPort.prepare() = pose;
1900  gazeboMoverPort.writeStrict();
1901  return true;
1902  }
1903 
1904  }
1905  return false;
1906  }
1907 
1908  void startDemo(const Vector& lookat)
1909  {
1910  if (lookat.length() == 3)
1911  {
1912  gazeCtrl->lookAtAbsAnglesSync(lookat);
1913  gazeCtrl->waitMotionDone(.1, 5.);
1914  }
1915  go=true;
1916  }
1917 
1918  void stopDemo()
1919  {
1920  go=false;
1921  stopControl();
1922  Time::delay(1.0);
1923  steerTorsoToHome();
1924  steerHeadToHome();
1925  steerArmToHome(LEFTARM);
1926  steerArmToHome(RIGHTARM);
1927  wentHome=true;
1928  deleteGuiTarget();
1929  if(useSpeech) sendSpeak(speech_idle[(int)Rand::scalar(0,speech_idle.size()-1e-3)]);
1930  state=STATE_IDLE;
1931  }
1932 
1933  void run()
1934  {
1935  if (go)
1936  {
1937  getSensorData();
1938  doIdle();
1939  commandHead();
1940  selectArm();
1941  doReach();
1942  if (((armSel==LEFTARM) && leftGraspEnable) ||
1943  ((armSel==RIGHTARM) && rightGraspEnable))
1944  {
1945  doGrasp();
1946  doRelease();
1947  doWait();
1948  }
1949 
1950  commandFace();
1951  }
1952  }
1953 
1954  void threadRelease()
1955  {
1956  stopControl();
1957  steerTorsoToHome();
1958  steerHeadToHome();
1959  steerArmToHome(LEFTARM);
1960  steerArmToHome(RIGHTARM);
1961 
1962  checkTorsoHome(3.0);
1963  checkArmHome(LEFTARM,3.0);
1964  checkArmHome(RIGHTARM,3.0);
1965 
1966  if (useLeftArm)
1967  {
1968  ICartesianControl *icart;
1969  drvCartLeftArm->view(icart);
1970  icart->restoreContext(startup_context_id_left);
1971 
1972  if (leftArmImpVelMode)
1973  {
1974  IInteractionMode *imode;
1975  drvLeftArm->view(imode);
1976 
1977  int len=leftArmJointsStiffness.length()<leftArmJointsDamping.length()?
1978  leftArmJointsStiffness.length():leftArmJointsDamping.length();
1979 
1980  for (int j=0; j<len; j++)
1981  imode->setInteractionMode(j,VOCAB_IM_STIFF);
1982  }
1983  }
1984 
1985  if (useRightArm)
1986  {
1987  ICartesianControl *icart;
1988  drvCartRightArm->view(icart);
1989  icart->restoreContext(startup_context_id_right);
1990 
1991  if (rightArmImpVelMode)
1992  {
1993  IInteractionMode *imode;
1994  drvRightArm->view(imode);
1995 
1996  int len=rightArmJointsStiffness.length()<rightArmJointsDamping.length()?
1997  rightArmJointsStiffness.length():rightArmJointsDamping.length();
1998 
1999  for (int j=0; j<len; j++)
2000  imode->setInteractionMode(j,VOCAB_IM_STIFF);
2001  }
2002  }
2003 
2004  gazeCtrl->restoreContext(startup_context_id_gaze);
2005 
2006  close();
2007  }
2008 };
2009 
2010 
2011 class managerModule: public RFModule
2012 {
2013 protected:
2014  managerThread *thr;
2015  Port rpcPort;
2016 
2017 public:
2018  managerModule() { }
2019 
2020  bool configure(ResourceFinder &rf)
2021  {
2022  thr=new managerThread(getName(),rf);
2023  if (!thr->start())
2024  {
2025  delete thr;
2026  return false;
2027  }
2028 
2029  rpcPort.open(getName("/rpc"));
2030  attach(rpcPort);
2031 
2032  return true;
2033  }
2034 
2035  bool close()
2036  {
2037  rpcPort.interrupt();
2038  rpcPort.close();
2039 
2040  thr->stop();
2041  delete thr;
2042 
2043  return true;
2044  }
2045 
2046  bool respond(const Bottle& cmd, Bottle& reply) override
2047  {
2048  if (cmd.get(0).asString() == "update_pose")
2049  {
2050  if (cmd.size()<4)
2051  {
2052  yError() << "Requires x y z";
2053  reply.addVocab32("fail");
2054  return false;
2055  }
2056  double x=cmd.get(1).asFloat64();
2057  double y=cmd.get(2).asFloat64();
2058  double z=cmd.get(3).asFloat64();
2059  bool ok=thr->updateBall(x,y,z);
2060  if (ok)
2061  {
2062  reply.addVocab32("ok");
2063  }
2064  else
2065  {
2066  reply.addVocab32("fail");
2067  }
2068  }
2069  if (cmd.get(0).asString() == "start")
2070  {
2071  Vector lookat;
2072  if (cmd.size() >=4)
2073  {
2074  lookat.resize(3);
2075  lookat[0]=cmd.get(1).asFloat64();
2076  lookat[1]=cmd.get(2).asFloat64();
2077  lookat[2]=cmd.get(3).asFloat64();
2078  }
2079  thr->startDemo(lookat);
2080  reply.addVocab32("ok");
2081  }
2082  if (cmd.get(0).asString() == "stop")
2083  {
2084  thr->stopDemo();
2085  reply.addVocab32("ok");
2086  }
2087  return true;
2088  }
2089 
2090  double getPeriod() { return 1.0; }
2091  bool updateModule() { return true; }
2092 };
2093 
2094 
2095 int main(int argc, char *argv[])
2096 {
2097  Network yarp;
2098  if (!yarp.checkNetwork())
2099  {
2100  yError("YARP server not available!");
2101  return 1;
2102  }
2103 
2104  ResourceFinder rf;
2105  rf.setDefaultContext("demoRedBall");
2106  rf.setDefaultConfigFile("config.ini");
2107  rf.configure(argc,argv);
2108 
2109  managerModule mod;
2110  mod.setName("/demoRedBall");
2111 
2112  return mod.runModule(rf);
2113 }
Generated by doxygen 1.9.1