iCub-main
actionPrimitives.cpp
Go to the documentation of this file.
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 
19 #include <cstdio>
20 #include <cstdarg>
21 #include <sstream>
22 #include <cmath>
23 #include <limits>
24 #include <algorithm>
25 
26 #include <yarp/math/Math.h>
27 #include <yarp/math/Rand.h>
28 
29 #include <iCub/ctrl/math.h>
33 
34 #define RES_WAVER(x) (dynamic_cast<ArmWavingMonitor*>(x))
35 
36 #define ACTIONPRIM_DEFAULT_PER 50 // [ms]
37 #define ACTIONPRIM_DEFAULT_EXECTIME 2.0 // [s]
38 #define ACTIONPRIM_DEFAULT_REACHTOL 0.005 // [m]
39 #define ACTIONPRIM_DUMP_PERIOD 1.0 // [s]
40 #define ACTIONPRIM_DEFAULT_PART "right_arm"
41 #define ACTIONPRIM_DEFAULT_TRACKINGMODE "off"
42 #define ACTIONPRIM_DEFAULT_VERBOSITY "off"
43 #define ACTIONPRIM_DEFAULT_WBDYN_STEMNAME "wholeBodyDynamics"
44 #define ACTIONPRIM_DEFAULT_WBDYN_PORTNAME "cartesianEndEffectorWrench:o"
45 
46 // defines for balancing the arm when in home position
47 #define ACTIONPRIM_BALANCEARM_PERIOD 2.0 // [s]
48 #define ACTIONPRIM_BALANCEARM_LENGTH 0.03 // [m]
49 
50 #define ACTIONPRIM_TORSO_PITCH "torso_pitch"
51 #define ACTIONPRIM_TORSO_ROLL "torso_roll"
52 #define ACTIONPRIM_TORSO_YAW "torso_yaw"
53 
54 using namespace std;
55 using namespace yarp::os;
56 using namespace yarp::dev;
57 using namespace yarp::sig;
58 using namespace yarp::math;
59 using namespace iCub::ctrl;
60 using namespace iCub::perception;
61 using namespace iCub::action;
62 
63 namespace iCub { namespace action { namespace log {
64  enum { no_info, info, warning, error };
65 } } }
66 
67 namespace iCub
68 {
69 
70 namespace action
71 {
72 
73 // This class handles the arm way points
74 /************************************************************************/
75 class ArmWayPoints : public PeriodicThread
76 {
77  deque<ActionPrimitivesWayPoint> wayPoints;
78  ActionPrimitives *action;
79  ICartesianControl *cartCtrl;
80  double default_exec_time;
81  bool firstRun;
82  double t0;
83  Vector x0;
84  size_t i;
85 
86  /************************************************************************/
87  double checkTime(const double time) const
88  {
89  return std::max(time,0.01);
90  }
91 
92  /************************************************************************/
93  double checkDefaultTime(const double time) const
94  {
95  return (time>0.0?time:default_exec_time);
96  }
97 
98  /************************************************************************/
99  void printWayPoint()
100  {
101  if (wayPoints[i].oEnabled)
102  action->printMessage(log::no_info,"reaching waypoint(%d): x=[%s]; o=[%s]",i,
103  wayPoints[i].x.toString(3,3).c_str(),
104  wayPoints[i].o.toString(3,3).c_str());
105  else
106  action->printMessage(log::no_info,"reaching waypoint(%d): x=[%s]",i,
107  wayPoints[i].x.toString(3,3).c_str());
108  }
109 
110  /************************************************************************/
111  void execCallback()
112  {
113  if (wayPoints[i].callback!=NULL)
114  {
115  action->printMessage(log::no_info,"executing waypoint(%d)-end callback ...",i);
116  wayPoints[i].callback->exec();
117  action->printMessage(log::no_info,"... waypoint(%d)-end callback executed",i);
118  }
119  }
120 
121 public:
122  /************************************************************************/
123  ArmWayPoints(ActionPrimitives *_action, const deque<ActionPrimitivesWayPoint> &_wayPoints) :
124  PeriodicThread((double)ACTIONPRIM_DEFAULT_PER/1000.0)
125  {
126  action=_action;
127  action->getCartesianIF(cartCtrl);
128  wayPoints=_wayPoints;
129  default_exec_time=ACTIONPRIM_DEFAULT_EXECTIME;
130  firstRun=true;
131  }
132 
133  /************************************************************************/
134  void set_default_exec_time(const double exec_time)
135  {
136  default_exec_time=exec_time;
137  }
138 
139  /************************************************************************/
140  bool threadInit()
141  {
142  if ((cartCtrl!=NULL) && (wayPoints.size()>0))
143  {
144  Vector o;
145  cartCtrl->getPose(x0,o);
146  setPeriod(checkTime(wayPoints[0].granularity));
147  i=0;
148 
149  return true;
150  }
151  else
152  return false;
153  }
154 
155  /************************************************************************/
156  void run()
157  {
158  if (firstRun)
159  {
160  printWayPoint();
161  firstRun=false;
162  t0=Time::now();
163  }
164 
165  double t=Time::now()-t0;
166  double trajTime=checkDefaultTime(wayPoints[i].trajTime);
167  double r=t/checkTime(checkDefaultTime(wayPoints[i].duration));
168 
169  Vector x=(r<1.0)?(x0+r*(wayPoints[i].x-x0)):wayPoints[i].x;
170  if (wayPoints[i].oEnabled)
171  cartCtrl->goToPose(x,wayPoints[i].o,trajTime);
172  else
173  cartCtrl->goToPosition(x,trajTime);
174 
175  // waypoint attained
176  if (r>=1.0)
177  {
178  execCallback();
179  if (i<wayPoints.size()-1)
180  {
181  x0=wayPoints[i].x;
182  i++;
183  printWayPoint();
184  setPeriod(checkTime(wayPoints[i].granularity));
185  t0=Time::now();
186  }
187  else
188  askToStop();
189  }
190  }
191 
192  /************************************************************************/
193  virtual ~ArmWayPoints()
194  {
195  if (isRunning())
196  stop();
197  }
198 };
199 
200 
201 // This class handles the automatic arm-waving
202 /************************************************************************/
203 class ArmWavingMonitor : public PeriodicThread
204 {
205  ICartesianControl *cartCtrl;
206  Vector restPos, restOrien;
207  double trajTime;
208 
209 public:
210  /************************************************************************/
211  ArmWavingMonitor(ICartesianControl *_cartCtrl) :
212  PeriodicThread(ACTIONPRIM_BALANCEARM_PERIOD)
213  {
214  cartCtrl=_cartCtrl;
215  Rand::init();
216  }
217 
218  /************************************************************************/
219  void setRestPosition(const Vector &_restPos)
220  {
221  restPos=_restPos;
222  }
223 
224  /************************************************************************/
225  void afterStart(bool success)
226  {
227  cartCtrl->getTrajTime(&trajTime);
228 
229  // start in suspended mode
230  disable();
231  }
232 
233  /************************************************************************/
234  bool enable()
235  {
236  if (isSuspended())
237  {
238  // try to keep the original orientation
239  Vector xdhat,qdhat;
240  cartCtrl->getPose(xdhat,restOrien);
241  cartCtrl->askForPose(restPos,restOrien,xdhat,restOrien,qdhat);
242 
243  cartCtrl->getTrajTime(&trajTime);
244  cartCtrl->setTrajTime(ACTIONPRIM_BALANCEARM_PERIOD);
245 
246  resume();
247  return true;
248  }
249  else
250  return false;
251  }
252 
253  /************************************************************************/
254  bool disable()
255  {
256  if (!isSuspended())
257  {
258  suspend();
259  cartCtrl->setTrajTime(trajTime);
260  return true;
261  }
262  else
263  return false;
264  }
265 
266  /************************************************************************/
267  void run()
268  {
269  size_t len=restPos.length();
270  if ((cartCtrl!=NULL) && (len>=3))
271  {
272  Vector halves(len,0.5);
273  Vector randOffs=ACTIONPRIM_BALANCEARM_LENGTH*(Rand::vector(len)-halves);
274 
275  cartCtrl->goToPose(restPos+randOffs,restOrien);
276  }
277  }
278 
279  /************************************************************************/
281  {
282  // safety check: make sure to stop
283  // the interface when closing
284  cartCtrl->stopControl();
285  }
286 };
287 
288 }
289 
290 }
291 
292 
293 /************************************************************************/
294 ActionPrimitivesWayPoint::ActionPrimitivesWayPoint()
295 {
296  x.resize(3,0.0);
297  o.resize(4,0.0);
298  oEnabled=false;
301  granularity=(double)ACTIONPRIM_DEFAULT_PER/1000.0;
302  callback=NULL;
303 }
304 
305 
306 /************************************************************************/
307 void ActionPrimitives::ActionsQueue::clear()
308 {
309  for (size_t i=0; i<this->size(); i++)
310  {
311  Action &action=(*this)[i];
312  if (action.execWayPoints)
313  delete action.wayPointsThr;
314  }
315 
316  deque<Action>::clear();
317 }
318 
319 
320 /************************************************************************/
321 ActionPrimitives::ActionPrimitives() :
322  PeriodicThread((double)ACTIONPRIM_DEFAULT_PER/1000.0)
323 {
324  init();
325 }
326 
327 
328 /************************************************************************/
329 ActionPrimitives::ActionPrimitives(Property &opt) :
330  PeriodicThread((double)ACTIONPRIM_DEFAULT_PER/1000.0)
331 {
332  init();
333  open(opt);
334 }
335 
336 
337 /************************************************************************/
339 {
340  armWaver=NULL;
341  actionClb=NULL;
342  actionWP=NULL;
343  graspModel=NULL;
344 
347  configured=closed=false;
348  checkEnabled=true;
349  torsoActive=true;
350  handSeqTerminator=false;
351  fingersInPosition=true;
352  fingerInPosition.insert(fingerInPosition.begin(),5,true);
353  reachTmoEnabled=false;
354  locked=false;
355 
359  latchTimerReachLog=0.0;
360 }
361 
362 
363 /************************************************************************/
365 {
366  return configured;
367 }
368 
369 
370 /************************************************************************/
371 void ActionPrimitives::printMessage(const int logtype, const char *format, ...) const
372 {
373  if (verbose)
374  {
375  string str;
376  str="*** "+local+"/"+part+": ";
377 
378  va_list arg;
379  char buf[512];
380  va_start(arg,format);
381  vsnprintf(buf,sizeof(buf),format,arg);
382  va_end(arg);
383 
384  str+=buf;
385  switch (logtype)
386  {
387  case log::error:
388  yError()<<str;
389  break;
390  case log::warning:
391  yWarning()<<str;
392  break;
393  case log::info:
394  yInfo()<<str;
395  break;
396  default:
397  printf("%s\n",str.c_str());
398  }
399  }
400 }
401 
402 
403 /************************************************************************/
404 bool ActionPrimitives::handleTorsoDOF(Property &opt, const string &key)
405 {
406  if (opt.check(key))
407  {
408  Bottle info;
409  cartCtrl->getInfo(info);
410  double hwver=info.find("arm_version").asFloat64();
411  map<string,int> remap{{ACTIONPRIM_TORSO_PITCH,0},{ACTIONPRIM_TORSO_ROLL,1},{ACTIONPRIM_TORSO_YAW,2}};
412  if (hwver>=3.0)
413  {
414  remap[ACTIONPRIM_TORSO_ROLL]=0;
415  remap[ACTIONPRIM_TORSO_PITCH]=1;
416  }
417  const int j=remap[key];
418 
419  bool sw=opt.find(key).asString()=="on"?true:false;
420 
421  Vector newDof(3,2.0), dummyRet;
422  newDof[j]=sw?1.0:0.0;
423 
424  printMessage(log::no_info,"%s %s",key.c_str(),sw?"enabled":"disabled");
425  cartCtrl->setDOF(newDof,dummyRet);
426 
427  if (sw)
428  {
429  string minKey=key+"_min";
430  string maxKey=key+"_max";
431  double min, max;
432 
433  cartCtrl->getLimits(j,&min,&max);
434 
435  min=opt.check(minKey,Value(min)).asFloat64();
436  max=opt.check(maxKey,Value(max)).asFloat64();
437 
438  cartCtrl->setLimits(j,min,max);
439  cartCtrl->getLimits(j,&min,&max);
440 
441  Vector weights;
442  if (cartCtrl->getRestWeights(weights))
443  {
444  // enforce the torso rest position
445  weights[j]=4.0;
446  cartCtrl->setRestWeights(weights,weights);
447  }
448  else
449  {
450  printMessage(log::error,"failed to get weights from cartesian controller");
451  return false;
452  }
453 
454  printMessage(log::no_info,"%s limits: [%g,%g] deg; weight=%g",key.c_str(),min,max,weights[j]);
455  }
456 
457  return true;
458  }
459 
460  return false;
461 }
462 
463 
464 /************************************************************************/
466 {
467  if (opt.check("hand_sequences_file"))
468  {
469  string handSeqFile=opt.find("hand_sequences_file").asString();
470 
471  printMessage(log::info,"Processing %s file",handSeqFile.c_str());
472  Property handSeqProp; handSeqProp.fromConfigFile(handSeqFile);
473 
474  // GENERAL group
475  Bottle &bGeneral=handSeqProp.findGroup("GENERAL");
476  if (bGeneral.isNull())
477  {
478  printMessage(log::warning,"\"GENERAL\" group is missing");
479  return false;
480  }
481 
482  if (!bGeneral.check("numSequences"))
483  {
484  printMessage(log::warning,"\"numSequences\" option is missing");
485  return false;
486  }
487 
488  int numSequences=bGeneral.find("numSequences").asInt32();
489 
490  // SEQUENCE groups
491  for (int i=0; i<numSequences; i++)
492  {
493  ostringstream seq;
494  seq<<"SEQ_"<<i;
495 
496  Bottle &bSeq=handSeqProp.findGroup(seq.str());
497  if (bSeq.isNull())
498  {
499  printMessage(log::warning,"\"%s\" group is missing",seq.str().c_str());
500  return false;
501  }
502 
503  if (!bSeq.check("key"))
504  {
505  printMessage(log::warning,"\"key\" option is missing");
506  return false;
507  }
508 
509  string key=bSeq.find("key").asString();
510 
511  if (isValidHandSeq(key))
512  {
513  printMessage(log::warning,"the sequence \"%s\" is already defined: skipping ...",
514  key.c_str());
515  continue;
516  }
517 
518  addHandSequence(key,bSeq);
519  }
520 
521  return true;
522  }
523  else
524  return false;
525 }
526 
527 
528 /************************************************************************/
530 {
531  bool ret=false;
532  if (opt.check("grasp_model_type"))
533  {
534  string modelType=opt.find("grasp_model_type").asString();
535  if (modelType!="none")
536  {
537  if (opt.check("grasp_model_file"))
538  {
539  if (modelType=="springy")
541  else if (modelType=="tactile")
543  else
544  {
545  printMessage(log::warning,"unknown grasp model type %s!",modelType.c_str());
546  return false;
547  }
548 
549  string modelFile=opt.find("grasp_model_file").asString();
550  printMessage(log::info,"Retrieving grasp model data from %s file",modelFile.c_str());
551  Property modelProp; modelProp.fromConfigFile(modelFile);
552 
553  // consistency check between the model and the part
554  if (modelProp.check("type"))
555  {
556  string type=modelProp.find("type").asString();
557  type+="_arm";
558  if (type!=part)
559  {
560  printMessage(log::warning,"attempt to instantiate a grasp model of type %s while part is %s",
561  type.c_str(),part.c_str());
562  return false;
563  }
564  }
565 
566  // override some information
567  modelProp.put("robot",robot);
568  return graspModel->fromProperty(modelProp);
569  }
570  else
571  printMessage(log::warning,"unable to find \"grasp_model_file\" option!");
572  }
573  else
574  ret=true;
575  }
576 
577  return ret;
578 }
579 
580 
581 /************************************************************************/
582 bool ActionPrimitives::open(Property &opt)
583 {
584  if (configured)
585  {
586  printMessage(log::warning,"already configured");
587  return true;
588  }
589 
590  if (!opt.check("local"))
591  {
592  printMessage(log::error,"option \"local\" is missing");
593  return false;
594  }
595 
596  robot=opt.check("robot",Value("icub")).asString();
597  local=opt.find("local").asString();
598  part=opt.check("part",Value(ACTIONPRIM_DEFAULT_PART)).asString();
599  default_exec_time=opt.check("default_exec_time",Value(ACTIONPRIM_DEFAULT_EXECTIME)).asFloat64();
600  tracking_mode=opt.check("tracking_mode",Value(ACTIONPRIM_DEFAULT_TRACKINGMODE)).asString()=="on"?true:false;
601  verbose=opt.check("verbosity",Value(ACTIONPRIM_DEFAULT_VERBOSITY)).asString()=="on"?true:false;
602 
603  int period=opt.check("thread_period",Value(ACTIONPRIM_DEFAULT_PER)).asInt32();
604  double reach_tol=opt.check("reach_tol",Value(ACTIONPRIM_DEFAULT_REACHTOL)).asFloat64();
605 
606  // create the model for grasp detection (if any)
607  if (!configGraspModel(opt))
608  {
609  close();
610  return false;
611  }
612 
613  // get hand sequence motions (if any)
614  configHandSeq(opt);
615 
616  // open the position client
617  Property optPolyHand("(device remote_controlboard)");
618  optPolyHand.put("remote","/"+robot+"/"+part);
619  optPolyHand.put("local","/"+local+"/"+part+"/position");
620  if (!polyHand.open(optPolyHand))
621  {
622  close();
623  return false;
624  }
625 
626  // open the cartesian client
627  Property optPolyCart("(device cartesiancontrollerclient)");
628  optPolyCart.put("remote","/"+robot+"/cartesianController/"+part);
629  optPolyCart.put("local","/"+local+"/"+part+"/cartesian");
630  if (!polyCart.open(optPolyCart))
631  {
632  close();
633  return false;
634  }
635 
636  // open views
637  polyHand.view(modCtrl);
638  polyHand.view(encCtrl);
639  polyHand.view(posCtrl);
640  polyCart.view(cartCtrl);
641 
642  // set tolerance
643  cartCtrl->setInTargetTol(reach_tol);
644 
645  // set tracking mode
647 
648  // handle torso DOF's
650  return false;
652  return false;
654  return false;
655 
656  Vector curDof;
657  cartCtrl->getDOF(curDof);
658 
659  enableTorsoSw.resize(3,0.0);
660  disableTorsoSw.resize(3,0.0);
661  for (int i=0; i<3; i++)
662  enableTorsoSw[i]=curDof[i];
663 
664  // start with torso disabled
665  disableTorsoDof();
666 
667  jHandMin=7; // hand first joint
668  posCtrl->getAxes(&jHandMax); // hand last joint
669 
670  // hand joints set
671  for (int j=jHandMin; j<jHandMax; j++)
672  {
673  fingersJnts.push_back(j);
674  fingersJntsSet.insert(j);
675  }
676 
677  // map from hand joints to fingers
678  fingers2JntsMap.insert(pair<int,int>(0,8));
679  fingers2JntsMap.insert(pair<int,int>(0,9));
680  fingers2JntsMap.insert(pair<int,int>(0,10));
681  fingers2JntsMap.insert(pair<int,int>(1,11));
682  fingers2JntsMap.insert(pair<int,int>(1,12));
683  fingers2JntsMap.insert(pair<int,int>(2,13));
684  fingers2JntsMap.insert(pair<int,int>(2,14));
685  fingers2JntsMap.insert(pair<int,int>(3,15));
686  fingers2JntsMap.insert(pair<int,int>(4,15));
687 
688  // start the thread with the specified period
689  setPeriod((double)period/1000.0);
690  start();
691 
692  // start the balancer thread
694  armWaver->start();
695 
696  return configured=true;
697 }
698 
699 
700 /************************************************************************/
702 {
703  if (closed)
704  return;
705 
706  if (armWaver!=NULL)
707  {
708  printMessage(log::info,"stopping balancer thread ...");
709  armWaver->stop();
710 
711  delete armWaver;
712  armWaver=NULL;
713  }
714 
715  if (isRunning())
716  {
717  printMessage(log::info,"stopping main thread ...");
718  stop();
719  }
720 
721  if (polyHand.isValid() && polyCart.isValid())
722  stopControl();
723 
724  if (polyHand.isValid())
725  {
726  printMessage(log::info,"closing hand driver ...");
727  polyHand.close();
728  }
729 
730  if (polyCart.isValid())
731  {
732  printMessage(log::info,"closing cartesian driver ...");
733  polyCart.close();
734  }
735 
736  delete graspModel;
737  graspModel=NULL;
738 
740 
741  closed=true;
742 }
743 
744 
745 /************************************************************************/
747 {
748  // latch the current moving fingers set
749  set<int> tmpSet=fingersMovingJntsSet;
750 
751  for (set<int>::iterator i=fingersMovingJntsSet.begin(); i!=fingersMovingJntsSet.end(); ++i)
752  {
753  if (handCheckMotionDone(*i))
754  tmpSet.erase(*i);
755  }
756 
757  // get data from the grasp model
758  if (graspModel!=NULL)
759  {
760  Value out; graspModel->getOutput(out);
761  Bottle *pB=out.asList();
762 
763  // span over fingers
764  for (int fng=0; fng<5; fng++)
765  {
766  double val=pB->get(fng).asFloat64();
767  double thres=curGraspDetectionThres[fng];
768 
769  // detect contact on the finger
770  if (val>thres)
771  {
772  fingersInPosition=false;
773  fingerInPosition[fng]=false;
774 
775  // take joints belonging to the finger
776  pair<multimap<int,int>::iterator,multimap<int,int>::iterator> i=fingers2JntsMap.equal_range(fng);
777 
778  for (multimap<int,int>::iterator j=i.first; j!=i.second; ++j)
779  {
780  int jnt=j->second;
781 
782  // stop and remove if not done yet
783  if (tmpSet.find(jnt)!=tmpSet.end())
784  {
785  printMessage(log::info,"contact detected on finger %d: (%g>%g) => stopping joint %d",
786  fng,val,thres,jnt);
787 
788  posCtrl->stop(jnt);
789  tmpSet.erase(jnt);
790  }
791  }
792  }
793  }
794  }
795 
796  // handle hand timeout
797  if ((Time::now()-latchTimerHand)>curHandTmo)
798  {
799  printMessage(log::info,"timeout (%g) expired on hand WP",curHandTmo);
800 
801  for (set<int>::iterator i=fingersMovingJntsSet.begin(); i!=fingersMovingJntsSet.end(); ++i)
802  {
803  posCtrl->stop(*i);
804  tmpSet.erase(*i);
805  }
806  }
807 
808  // update the moving fingers set
809  fingersMovingJntsSet=tmpSet;
810 
811  return (fingersMovingJntsSet.size()==0);
812 }
813 
814 
815 /************************************************************************/
817 {
819 }
820 
821 
822 /************************************************************************/
824 {
825  if (configured)
826  {
827  lock_guard<mutex> lck(mtx);
829  return true;
830  }
831  else
832  return false;
833 }
834 
835 
836 /************************************************************************/
838 {
839  if (configured)
840  {
841  locked=true;
842  return true;
843  }
844  else
845  return false;
846 }
847 
848 
849 /************************************************************************/
851 {
852  if (configured)
853  {
854  locked=false;
855  return true;
856  }
857  else
858  return false;
859 }
860 
861 
862 /************************************************************************/
864 {
865  return locked;
866 }
867 
868 
869 /************************************************************************/
870 bool ActionPrimitives::_pushAction(const bool execArm, const Vector &x, const Vector &o,
871  const double execTime, const bool oEnabled, const bool execHand,
872  const HandWayPoint &handWP, const bool handSeqTerminator,
874 {
875  if (configured && !locked)
876  {
877  lock_guard<mutex> lck(mtx);
878  Action action;
879 
880  action.waitState=false;
881  action.execArm=execArm;
882  action.x=x;
883  action.o=o;
884  action.execTime=execTime;
885  action.oEnabled=oEnabled;
886  action.execHand=execHand;
887  action.handWP=handWP;
888  action.handSeqTerminator=handSeqTerminator;
889  action.execWayPoints=false;
890  action.clb=clb;
891 
892  actionsQueue.push_back(action);
893  return true;
894  }
895  else
896  return false;
897 }
898 
899 
900 /************************************************************************/
901 bool ActionPrimitives::_pushAction(const Vector &x, const Vector &o,
902  const string &handSeqKey, const double execTime,
903  ActionPrimitivesCallback *clb, const bool oEnabled)
904 {
905  if (configured)
906  {
907  map<string,deque<HandWayPoint> >::iterator itr=handSeqMap.find(handSeqKey);
908  if (itr!=handSeqMap.end())
909  {
910  deque<HandWayPoint> &q=itr->second;
911  if (q.size()>0)
912  {
913  Vector vectDummy(1);
914 
915  // combined action
916  _pushAction(true,x,o,execTime,oEnabled,true,q[0],q.size()==1,q.size()==1?clb:NULL);
917 
918  if (q.size()>1)
919  {
920  size_t i;
921 
922  // decompose hand action in sum of fingers sequences
923  for (i=1; i<q.size()-1; i++)
924  _pushAction(false,vectDummy,vectDummy,ACTIONPRIM_DISABLE_EXECTIME,false,true,q[i],false,NULL);
925 
926  // reserve the callback whenever the last hand WP is achieved
927  if (i<q.size())
928  _pushAction(false,vectDummy,vectDummy,ACTIONPRIM_DISABLE_EXECTIME,false,true,q[i],true,clb);
929  }
930  }
931 
932  return true;
933  }
934  else
935  {
936  printMessage(log::warning,"\"%s\" hand sequence key not found",
937  handSeqKey.c_str());
938 
939  return false;
940  }
941  }
942  else
943  return false;
944 }
945 
946 
947 /************************************************************************/
948 bool ActionPrimitives::pushAction(const Vector &x, const Vector &o,
949  const string &handSeqKey, const double execTime,
951 {
952  return _pushAction(x,o,handSeqKey,execTime,clb,true);
953 }
954 
955 
956 /************************************************************************/
957 bool ActionPrimitives::pushAction(const Vector &x, const string &handSeqKey,
958  const double execTime, ActionPrimitivesCallback *clb)
959 {
960  Vector vectDummy(1);
961  return _pushAction(x,vectDummy,handSeqKey,execTime,clb,false);
962 }
963 
964 
965 /************************************************************************/
966 bool ActionPrimitives::pushAction(const Vector &x, const Vector &o,
967  const double execTime,
969 {
970  if (configured)
971  {
972  HandWayPoint handDummy;
973 
974  _pushAction(true,x,o,execTime,true,false,handDummy,false,clb);
975 
976  return true;
977  }
978  else
979  return false;
980 }
981 
982 
983 /************************************************************************/
984 bool ActionPrimitives::pushAction(const Vector &x, const double execTime,
986 {
987  if (configured)
988  {
989  HandWayPoint handDummy;
990  Vector vectDummy(1);
991 
992  _pushAction(true,x,vectDummy,execTime,false,false,handDummy,false,clb);
993 
994  return true;
995  }
996  else
997  return false;
998 }
999 
1000 
1001 /************************************************************************/
1002 bool ActionPrimitives::pushAction(const string &handSeqKey,
1004 {
1005  if (configured)
1006  {
1007  map<string,deque<HandWayPoint> >::iterator itr=handSeqMap.find(handSeqKey);
1008  if (itr!=handSeqMap.end())
1009  {
1010  deque<HandWayPoint> &q=itr->second;
1011  Vector vectDummy(1);
1012  unsigned int i;
1013 
1014  // decompose hand action in sum of fingers sequences
1015  for (i=0; i<q.size()-1; i++)
1016  _pushAction(false,vectDummy,vectDummy,ACTIONPRIM_DISABLE_EXECTIME,false,true,q[i],false,NULL);
1017 
1018  // reserve the callback whenever the last hand WP is achieved
1019  if (i<q.size())
1020  _pushAction(false,vectDummy,vectDummy,ACTIONPRIM_DISABLE_EXECTIME,false,true,q[i],true,clb);
1021 
1022  return true;
1023  }
1024  else
1025  {
1026  printMessage(log::warning,"\"%s\" hand sequence key not found",
1027  handSeqKey.c_str());
1028 
1029  return false;
1030  }
1031  }
1032  else
1033  return false;
1034 }
1035 
1036 
1037 /************************************************************************/
1038 bool ActionPrimitives::pushAction(const deque<ActionPrimitivesWayPoint> &wayPoints,
1040 {
1041  if (configured && !locked)
1042  {
1043  lock_guard<mutex> lck(mtx);
1044  Action action;
1045  ArmWayPoints *thr=new ArmWayPoints(this,wayPoints);
1047 
1048  action.waitState=false;
1049  action.execArm=false;
1050  action.execHand=false;
1051  action.handSeqTerminator=false;
1052  action.execWayPoints=true;
1053  action.wayPointsThr=thr;
1054  action.clb=clb;
1055 
1056  actionsQueue.push_back(action);
1057  return true;
1058  }
1059  else
1060  return false;
1061 }
1062 
1063 
1064 /************************************************************************/
1065 bool ActionPrimitives::pushAction(const deque<ActionPrimitivesWayPoint> &wayPoints,
1066  const string &handSeqKey, ActionPrimitivesCallback *clb)
1067 {
1068  if (configured && !locked)
1069  {
1070  lock_guard<mutex> lck(mtx);
1071  map<string,deque<HandWayPoint> >::iterator itr=handSeqMap.find(handSeqKey);
1072  if (itr!=handSeqMap.end())
1073  {
1074  deque<HandWayPoint> &q=itr->second;
1075  if (q.size()>0)
1076  {
1077  // combined action
1078  Action action;
1079  ArmWayPoints *thr=new ArmWayPoints(this,wayPoints);
1081 
1082  action.waitState=false;
1083  action.execArm=false;
1084  action.execHand=true;
1085  action.handWP=q[0];
1086  action.handSeqTerminator=(q.size()==1);
1087  action.execWayPoints=true;
1088  action.wayPointsThr=thr;
1089  action.clb=(q.size()==1?clb:NULL);
1090 
1091  actionsQueue.push_back(action);
1092 
1093  if (q.size()>1)
1094  {
1095  Vector vectDummy(1);
1096  size_t i;
1097 
1098  // decompose hand action in sum of fingers sequences
1099  for (i=1; i<q.size()-1; i++)
1100  _pushAction(false,vectDummy,vectDummy,ACTIONPRIM_DISABLE_EXECTIME,false,true,q[i],false,NULL);
1101 
1102  // reserve the callback whenever the last hand WP is achieved
1103  if (i<q.size())
1104  _pushAction(false,vectDummy,vectDummy,ACTIONPRIM_DISABLE_EXECTIME,false,true,q[i],true,clb);
1105  }
1106  }
1107 
1108  return true;
1109  }
1110  else
1111  {
1112  printMessage(log::warning,"\"%s\" hand sequence key not found",
1113  handSeqKey.c_str());
1114  return false;
1115  }
1116  }
1117  else
1118  return false;
1119 }
1120 
1121 
1122 /************************************************************************/
1124 {
1125  if (configured && !locked)
1126  {
1127  lock_guard<mutex> lck(mtx);
1128  Action action;
1129 
1130  action.waitState=true;
1131  action.tmo=tmo;
1132  action.execArm=false;
1133  action.execHand=false;
1134  action.handSeqTerminator=false;
1135  action.execWayPoints=false;
1136  action.clb=clb;
1137 
1138  actionsQueue.push_back(action);
1139  return true;
1140  }
1141  else
1142  return false;
1143 }
1144 
1145 
1146 /************************************************************************/
1147 bool ActionPrimitives::reachPose(const Vector &x, const Vector &o,
1148  const double execTime)
1149 {
1150  if (configured && !locked)
1151  {
1152  disableArmWaving();
1153 
1154  const double t=execTime>0.0?execTime:default_exec_time;
1155 
1156  enableTorsoDof();
1157 
1158  cartCtrl->goToPose(x,o,t);
1159 
1160  printMessage(log::no_info,"reach at %g [s] for [%s], [%s]",t,
1161  x.toString(3,3).c_str(),
1162  o.toString(3,3).c_str());
1163 
1165 
1166  latchTimerReachLog=latchTimerReach=Time::now();
1167 
1168  return true;
1169  }
1170  else
1171  return false;
1172 }
1173 
1174 
1175 /************************************************************************/
1176 bool ActionPrimitives::reachPosition(const Vector &x, const double execTime)
1177 {
1178  if (configured && !locked)
1179  {
1180  disableArmWaving();
1181 
1182  const double t=execTime>0.0?execTime:default_exec_time;
1183 
1184  enableTorsoDof();
1185 
1186  cartCtrl->goToPosition(x,t);
1187 
1188 
1189  printMessage(log::no_info,"reach at %g [s] for [%s]",t,
1190  x.toString(3,3).c_str());
1191 
1193 
1194  latchTimerReachLog=latchTimerReach=Time::now();
1195 
1196  return true;
1197  }
1198  else
1199  return false;
1200 }
1201 
1202 
1203 /************************************************************************/
1205 {
1206  bool exec=false;
1207  Action action;
1208 
1209  mtx.lock();
1210  if (actionsQueue.size()>0)
1211  {
1212  action=actionsQueue.front();
1213  actionsQueue.pop_front();
1214  exec=true;
1215  }
1216  mtx.unlock();
1217 
1218  if (exec)
1219  {
1220  if (action.waitState)
1221  wait(action);
1222 
1223  if (action.execArm)
1224  cmdArm(action);
1225 
1226  if (action.execWayPoints)
1227  {
1228  actionWP=action.wayPointsThr;
1229  cmdArmWP(action);
1230  }
1231  else
1232  actionWP=NULL;
1233 
1234  if (action.execHand)
1235  cmdHand(action);
1236 
1237  actionClb=action.clb;
1238  }
1239 
1240  return exec;
1241 }
1242 
1243 
1244 /************************************************************************/
1246 {
1247  bool exec=false;
1248  Action action;
1249 
1250  lock_guard<mutex> lck(mtx);
1251  if (actionsQueue.size()>0)
1252  {
1253  // polling on the first action in the queue
1254  action=actionsQueue.front();
1255 
1256  // if it is an hand-action then execute and update queue
1257  if (action.execHand && !action.execArm && !action.waitState)
1258  {
1259  actionsQueue.pop_front();
1260  cmdHand(action);
1261  exec=true;
1262  }
1263  }
1264 
1265  return exec;
1266 }
1267 
1268 
1269 /************************************************************************/
1271 {
1272  const double t=Time::now();
1273 
1274  if (!armMoveDone)
1275  {
1276  Vector x,o,xdhat,odhat,qdhat;
1277  cartCtrl->getPose(x,o);
1278  cartCtrl->getDesired(xdhat,odhat,qdhat);
1279 
1281  {
1282  printMessage(log::no_info,"reaching... xdhat=[%s] |e|=%.3f [m]; odhat=[%s] |e|=%.3f",
1283  xdhat.toString(3,3).c_str(),norm(xdhat-x),
1284  odhat.toString(3,3).c_str(),norm(odhat-o));
1285 
1287  }
1288 
1289  if (actionWP!=NULL)
1290  {
1291  if (!actionWP->isRunning())
1292  {
1293  cartCtrl->checkMotionDone(&armMoveDone);
1294  delete actionWP;
1295  actionWP=NULL;
1296  }
1297  }
1298  else
1299  cartCtrl->checkMotionDone(&armMoveDone);
1300 
1301  // check if timeout has expired
1302  if (reachTmoEnabled && !armMoveDone)
1303  {
1304  if ((t-latchTimerReach)>reachTmo)
1305  {
1306  printMessage(log::info,"timeout (%g) expired while reaching",reachTmo);
1307  armMoveDone=true;
1308  }
1309  }
1310 
1311  if (armMoveDone)
1312  {
1313  printMessage(log::no_info,"reaching complete");
1314  disableTorsoDof();
1315  }
1316  }
1317 
1318  if (!handMoveDone)
1319  {
1320  // check whether all the remaining active joints have come
1321  // to a complete stop
1323  if (handMoveDone)
1324  {
1325  printMessage(log::no_info,"hand WP reached");
1326 
1327  if (!handSeqTerminator)
1328  if (execPendingHandSequences()) // here handMoveDone may switch false again
1329  cv_motionStartEvent.notify_all();
1330  }
1331  }
1332 
1335 
1337  {
1338  // execute action-end callback
1339  if (actionClb!=NULL)
1340  {
1341  printMessage(log::no_info,"executing action-end callback ...");
1342  actionClb->exec();
1343  printMessage(log::no_info,"... action-end callback executed");
1344 
1345  actionClb=NULL;
1346  }
1347 
1348  if (execQueuedAction())
1349  cv_motionStartEvent.notify_all();
1350  else
1351  cv_motionDoneEvent.notify_all();
1352  }
1353 }
1354 
1355 
1356 /************************************************************************/
1358 {
1359  close();
1360 }
1361 
1362 
1363 /************************************************************************/
1365 {
1366  double fb;
1367  if (encCtrl->getEncoder(jnt,&fb))
1368  return (fabs(curHandFinalPoss[jnt-jHandMin]-fb)<curHandTols[jnt-jHandMin]);
1369  else
1370  return false;
1371 }
1372 
1373 
1374 /************************************************************************/
1376 {
1377  // enable torso joints, if any
1378  if (!torsoActive && (norm(enableTorsoSw)>0.0))
1379  {
1380  Vector dummyRet;
1381  cartCtrl->setDOF(enableTorsoSw,dummyRet);
1382  torsoActive=true;
1383  }
1384 }
1385 
1386 
1387 /************************************************************************/
1389 {
1390  // disable torso joints, if any
1391  if (torsoActive && (norm(enableTorsoSw)>0.0))
1392  {
1393  Vector dummyRet;
1394  cartCtrl->setDOF(disableTorsoSw,dummyRet);
1395  torsoActive=false;
1396  }
1397 }
1398 
1399 
1400 /************************************************************************/
1401 bool ActionPrimitives::wait(const Action &action)
1402 {
1403  if (configured)
1404  {
1405  printMessage(log::no_info,"wait for %g seconds",action.tmo);
1406  waitTmo=action.tmo;
1407  latchTimerWait=Time::now();
1408 
1409  return true;
1410  }
1411  else
1412  return false;
1413 }
1414 
1415 
1416 /************************************************************************/
1418 {
1419  if (configured)
1420  {
1421  disableArmWaving();
1422 
1423  const Vector &x=action.x;
1424  const Vector &o=action.o;
1425  const bool oEnabled=action.oEnabled;
1426  const double t=action.execTime>0.0?action.execTime:default_exec_time;
1427 
1428  enableTorsoDof();
1429 
1430  if (oEnabled)
1431  {
1432  if (!cartCtrl->goToPoseSync(x,o,t))
1433  {
1434  printMessage(log::error,"reach log::error");
1435  return false;
1436  }
1437 
1438  printMessage(log::no_info,"reach at %g [s] for [%s], [%s]",t,
1439  x.toString(3,3).c_str(),
1440  o.toString(3,3).c_str());
1441  }
1442  else
1443  {
1444  if (!cartCtrl->goToPositionSync(x,t))
1445  {
1446  printMessage(log::error,"reach log::error");
1447  return false;
1448  }
1449 
1450  printMessage(log::no_info,"reach at %g [s] for [%s]",t,
1451  x.toString(3,3).c_str());
1452  }
1453 
1455 
1456  latchTimerReachLog=latchTimerReach=Time::now();
1457 
1458  return true;
1459  }
1460  else
1461  return false;
1462 }
1463 
1464 
1465 /************************************************************************/
1467 {
1468  if (configured && action.execWayPoints)
1469  {
1470  disableArmWaving();
1471  enableTorsoDof();
1472  action.wayPointsThr->start();
1474  latchTimerReachLog=latchTimerReach=Time::now();
1475 
1476  return true;
1477  }
1478  else
1479  return false;
1480 }
1481 
1482 
1483 /************************************************************************/
1485 {
1486  if (configured)
1487  {
1488  disableArmWaving();
1489 
1490  const string &tag=action.handWP.tag;
1491  const Vector &poss=action.handWP.poss;
1492  const Vector &vels=action.handWP.vels;
1493  const Vector &tols=action.handWP.tols;
1494  const Vector &thres=action.handWP.thres;
1495  const double &tmo=action.handWP.tmo;
1496 
1498  curHandFinalPoss=poss;
1499  curHandTols=tols;
1500  curGraspDetectionThres=thres;
1501  curHandTmo=tmo;
1502 
1503  size_t sz=std::min(fingersJnts.size(),std::min(poss.length(),vels.length()));
1504  for (size_t i=0; i<sz; i++)
1505  {
1506  int j=fingersJnts[i];
1507  modCtrl->setControlMode(j,VOCAB_CM_POSITION);
1508  posCtrl->setRefSpeed(j,vels[j-jHandMin]);
1509  }
1510 
1511  posCtrl->positionMove((int)sz,fingersJnts.data(),poss.data());
1512 
1515  fingersInPosition=true;
1516  fingerInPosition.clear();
1517  fingerInPosition.insert(fingerInPosition.begin(),5,true);
1518  printMessage(log::no_info,"\"%s\" WP: [%s] (thres = [%s]) (tmo = %g)",
1519  tag.c_str(),poss.toString(3,3).c_str(),
1520  thres.toString(3,3).c_str(),tmo);
1521 
1522  latchTimerHand=Time::now();
1523 
1524  return true;
1525  }
1526  else
1527  return false;
1528 }
1529 
1530 
1531 /************************************************************************/
1532 bool ActionPrimitives::addHandSeqWP(const string &handSeqKey, const Vector &poss,
1533  const Vector &vels, const Vector &tols, const Vector &thres,
1534  const double tmo)
1535 {
1536  if ((poss.length()==9) && (vels.length()==9) && (tols.length()==9) && (thres.length()==5))
1537  {
1538  HandWayPoint handWP;
1539 
1540  handWP.tag=handSeqKey;
1541  handWP.poss=poss;
1542  handWP.vels=vels;
1543  handWP.tols=tols;
1544  handWP.thres=thres;
1545  handWP.tmo=tmo;
1546 
1547  handSeqMap[handSeqKey].push_back(handWP);
1548 
1549  return true;
1550  }
1551  else
1552  return false;
1553 }
1554 
1555 
1556 /************************************************************************/
1557 bool ActionPrimitives::addHandSequence(const string &handSeqKey, const Bottle &sequence)
1558 {
1559  if (!sequence.check("numWayPoints"))
1560  {
1561  printMessage(log::warning,"\"numWayPoints\" option is missing");
1562  return false;
1563  }
1564 
1565  int numWayPoints=sequence.find("numWayPoints").asInt32();
1566  bool ret=false;
1567 
1568  for (int j=0; j<numWayPoints; j++)
1569  {
1570  ostringstream wp;
1571  wp<<"wp_"<<j;
1572 
1573  Bottle &bWP=sequence.findGroup(wp.str());
1574  if (bWP.isNull())
1575  {
1576  printMessage(log::warning,"\"%s\" entry is missing",wp.str().c_str());
1577  return false;
1578  }
1579 
1580  if (!bWP.check("poss"))
1581  {
1582  printMessage(log::warning,"\"poss\" option is missing");
1583  return false;
1584  }
1585 
1586  if (!bWP.check("vels"))
1587  {
1588  printMessage(log::warning,"\"vels\" option is missing");
1589  return false;
1590  }
1591 
1592  if (!bWP.check("tols"))
1593  {
1594  printMessage(log::warning,"\"tols\" option is missing");
1595  return false;
1596  }
1597 
1598  if (!bWP.check("thres"))
1599  {
1600  printMessage(log::warning,"\"thres\" option is missing");
1601  return false;
1602  }
1603 
1604  if (!bWP.check("tmo"))
1605  {
1606  printMessage(log::warning,"\"tmo\" option is missing");
1607  return false;
1608  }
1609 
1610  Bottle *bPoss=bWP.find("poss").asList();
1611  Vector poss(bPoss->size());
1612 
1613  for (size_t k=0; k<poss.length(); k++)
1614  poss[k]=bPoss->get(k).asFloat64();
1615 
1616  Bottle *bVels=bWP.find("vels").asList();
1617  Vector vels(bVels->size());
1618 
1619  for (size_t k=0; k<vels.length(); k++)
1620  vels[k]=bVels->get(k).asFloat64();
1621 
1622  Bottle *bTols=bWP.find("tols").asList();
1623  Vector tols(bTols->size());
1624 
1625  for (size_t k=0; k<tols.length(); k++)
1626  tols[k]=bTols->get(k).asFloat64();
1627 
1628  Bottle *bThres=bWP.find("thres").asList();
1629  Vector thres(bThres->size());
1630 
1631  for (size_t k=0; k<thres.length(); k++)
1632  thres[k]=bThres->get(k).asFloat64();
1633 
1634  double tmo=bWP.find("tmo").asFloat64();
1635 
1636  if (addHandSeqWP(handSeqKey,poss,vels,tols,thres,tmo))
1637  ret=true; // at least one WP has been added
1638  else
1639  printMessage(log::warning,"\"%s\" entry is invalid, not added to \"%s\"",
1640  wp.str().c_str(),handSeqKey.c_str());
1641  }
1642 
1643  return ret;
1644 }
1645 
1646 
1647 /************************************************************************/
1648 bool ActionPrimitives::isValidHandSeq(const string &handSeqKey)
1649 {
1650  map<string,deque<HandWayPoint> >::iterator itr=handSeqMap.find(handSeqKey);
1651 
1652  if (itr!=handSeqMap.end())
1653  return true;
1654  else
1655  return false;
1656 }
1657 
1658 
1659 /************************************************************************/
1660 bool ActionPrimitives::removeHandSeq(const string &handSeqKey)
1661 {
1662  map<string,deque<HandWayPoint> >::iterator itr=handSeqMap.find(handSeqKey);
1663 
1664  if (itr!=handSeqMap.end())
1665  {
1666  handSeqMap[handSeqKey].clear();
1667  return true;
1668  }
1669  else
1670  return false;
1671 }
1672 
1673 
1674 /************************************************************************/
1676 {
1677  map<string,deque<HandWayPoint> >::iterator itr;
1678  deque<string> q;
1679 
1680  for (itr=handSeqMap.begin(); itr!=handSeqMap.end(); ++itr)
1681  q.push_back(itr->first);
1682 
1683  return q;
1684 }
1685 
1686 
1687 /************************************************************************/
1688 bool ActionPrimitives::getHandSequence(const string &handSeqKey, Bottle &sequence)
1689 {
1690  if (isValidHandSeq(handSeqKey))
1691  {
1692  deque<HandWayPoint> &handWP=handSeqMap[handSeqKey];
1693  sequence.clear();
1694 
1695  // numWayPoints part
1696  Bottle &bNum=sequence.addList();
1697  bNum.addString("numWayPoints");
1698  bNum.addInt32((int)handWP.size());
1699 
1700  // wayPoints parts
1701  for (unsigned int i=0; i<handWP.size(); i++)
1702  {
1703  ostringstream wp;
1704  wp<<"wp_"<<i;
1705 
1706  Bottle &bWP=sequence.addList();
1707  bWP.addString(wp.str());
1708 
1709  // poss part
1710  Bottle &bPoss=bWP.addList();
1711  bPoss.addString("poss");
1712  Bottle &bPossVects=bPoss.addList();
1713  for (size_t j=0; j<handWP[i].poss.length(); j++)
1714  bPossVects.addFloat64(handWP[i].poss[j]);
1715 
1716  // vels part
1717  Bottle &bVels=bWP.addList();
1718  bVels.addString("vels");
1719  Bottle &bVelsVects=bVels.addList();
1720  for (size_t j=0; j<handWP[i].vels.length(); j++)
1721  bVelsVects.addFloat64(handWP[i].vels[j]);
1722 
1723  // tols part
1724  Bottle &bTols=bWP.addList();
1725  bTols.addString("tols");
1726  Bottle &bTolsVects=bTols.addList();
1727  for (size_t j=0; j<handWP[i].tols.length(); j++)
1728  bTolsVects.addFloat64(handWP[i].tols[j]);
1729 
1730  // thres part
1731  Bottle &bThres=bWP.addList();
1732  bThres.addString("thres");
1733  Bottle &bThresVects=bThres.addList();
1734  for (size_t j=0; j<handWP[i].thres.length(); j++)
1735  bThresVects.addFloat64(handWP[i].thres[j]);
1736 
1737  // tmo part
1738  Bottle &bTmo=bWP.addList();
1739  bTmo.addString("tmo");
1740  bTmo.addFloat64(handWP[i].tmo);
1741  }
1742 
1743  return true;
1744  }
1745  else
1746  return false;
1747 }
1748 
1749 
1750 /************************************************************************/
1752 {
1753  if (configured)
1754  {
1755  lock_guard<mutex> lck(mtx);
1757  return true;
1758  }
1759  else
1760  return false;
1761 }
1762 
1763 
1764 /************************************************************************/
1766 {
1767  if (configured)
1768  {
1769  lock_guard<mutex> lck(mtx);
1771  return true;
1772  }
1773  else
1774  return false;
1775 }
1776 
1777 
1778 /************************************************************************/
1780 {
1781  if (configured)
1782  {
1783  lock_guard<mutex> lck(mtx);
1785  return true;
1786  }
1787  else
1788  return false;
1789 }
1790 
1791 
1792 /************************************************************************/
1794 {
1795  if (configured)
1796  {
1797  model=static_cast<Model*>(graspModel);
1798  return true;
1799  }
1800  else
1801  return false;
1802 }
1803 
1804 
1805 /************************************************************************/
1806 bool ActionPrimitives::getCartesianIF(ICartesianControl *&ctrl) const
1807 {
1808  if (configured)
1809  {
1810  ctrl=cartCtrl;
1811  return true;
1812  }
1813  else
1814  return false;
1815 }
1816 
1817 
1818 /************************************************************************/
1820 {
1821  if (configured)
1822  {
1823  torso=enableTorsoSw;
1824  return true;
1825  }
1826  else
1827  return false;
1828 }
1829 
1830 
1831 /************************************************************************/
1832 bool ActionPrimitives::setTorsoJoints(const Vector &torso)
1833 {
1834  if (configured)
1835  {
1836  size_t len=std::min(torso.length(),size_t(3));
1837  for (size_t i=0; i<len; i++)
1838  enableTorsoSw[i]=torso[i];
1839 
1840  return true;
1841  }
1842  else
1843  return false;
1844 }
1845 
1846 
1847 /************************************************************************/
1848 bool ActionPrimitives::getPose(Vector &x, Vector &o) const
1849 {
1850  if (configured)
1851  {
1852  cartCtrl->getPose(x,o);
1853  return true;
1854  }
1855  else
1856  return false;
1857 }
1858 
1859 
1860 /************************************************************************/
1862 {
1863  if (configured)
1864  {
1865  suspend();
1866 
1867  if (actionWP!=NULL)
1868  {
1869  if (actionWP->isRunning())
1870  actionWP->stop();
1871 
1872  delete actionWP;
1873  actionWP=NULL;
1874  }
1875 
1877 
1878  cartCtrl->stopControl();
1879  posCtrl->stop((int)fingersJnts.size(),fingersJnts.data());
1880 
1883 
1884  resume();
1885 
1886  return true;
1887  }
1888  else
1889  return false;
1890 }
1891 
1892 
1893 /************************************************************************/
1894 bool ActionPrimitives::setDefaultExecTime(const double execTime)
1895 {
1896  if (configured && (execTime>0.0))
1897  {
1898  default_exec_time=execTime;
1899  return true;
1900  }
1901  else
1902  return false;
1903 }
1904 
1905 
1906 /************************************************************************/
1908 {
1909  return default_exec_time;
1910 }
1911 
1912 
1913 /************************************************************************/
1915 {
1916  if (configured)
1917  {
1918  if (cartCtrl->setTrackingMode(f))
1919  {
1920  tracking_mode=f;
1921  return true;
1922  }
1923  else
1924  return false;
1925  }
1926  else
1927  return false;
1928 }
1929 
1930 
1931 /************************************************************************/
1933 {
1934  return tracking_mode;
1935 }
1936 
1937 
1938 /************************************************************************/
1939 bool ActionPrimitives::enableArmWaving(const Vector &restPos)
1940 {
1941  if (configured)
1942  {
1943  RES_WAVER(armWaver)->setRestPosition(restPos);
1944  printMessage(log::no_info,"setting waving position to %s",
1945  restPos.toString(3,3).c_str());
1946 
1947  if (RES_WAVER(armWaver)->enable())
1948  printMessage(log::no_info,"arm waving enabled");
1949 
1950  return true;
1951  }
1952  else
1953  return false;
1954 }
1955 
1956 
1957 /************************************************************************/
1959 {
1960  if (configured)
1961  {
1962  if (RES_WAVER(armWaver)->disable())
1963  printMessage(log::no_info,"arm waving disabled");
1964 
1965  return true;
1966  }
1967  else
1968  return false;
1969 }
1970 
1971 
1972 /************************************************************************/
1974 {
1975  if (configured)
1976  {
1977  reachTmo=tmo;
1978  reachTmoEnabled=true;
1979 
1980  return true;
1981  }
1982  else
1983  return false;
1984 }
1985 
1986 
1987 /************************************************************************/
1989 {
1990  if (configured)
1991  {
1992  reachTmoEnabled=false;
1993  return true;
1994  }
1995  else
1996  return false;
1997 }
1998 
1999 
2000 /************************************************************************/
2001 bool ActionPrimitives::checkActionsDone(bool &f, const bool sync)
2002 {
2003  if (configured)
2004  {
2005  if (sync && checkEnabled)
2006  {
2007  unique_lock<mutex> lck(mtx_motionDoneEvent);
2008  cv_motionDoneEvent.wait(lck);
2009  }
2010 
2012 
2013  return true;
2014  }
2015  else
2016  return false;
2017 }
2018 
2019 
2020 /************************************************************************/
2021 bool ActionPrimitives::checkActionOnGoing(bool &f, const bool sync)
2022 {
2023  if (configured)
2024  {
2025  if (sync && checkEnabled)
2026  {
2027  unique_lock<mutex> lck(mtx_motionStartEvent);
2028  cv_motionStartEvent.wait(lck);
2029  }
2030 
2032 
2033  return true;
2034  }
2035  else
2036  return false;
2037 }
2038 
2039 
2040 /************************************************************************/
2042 {
2043  if (configured)
2044  {
2045  cv_motionDoneEvent.notify_all();
2046 
2047  if (disable)
2048  checkEnabled=false;
2049 
2050  return true;
2051  }
2052  else
2053  return false;
2054 }
2055 
2056 
2057 /************************************************************************/
2059 {
2060  if (configured)
2061  {
2062  checkEnabled=true;
2063  return true;
2064  }
2065  else
2066  return false;
2067 }
2068 
2069 
2070 /************************************************************************/
2071 bool ActionPrimitivesLayer1::grasp(const Vector &x, const Vector &o, const Vector &d)
2072 {
2073  if (configured)
2074  {
2075  printMessage(log::no_info,"start grasping");
2076 
2077  pushAction(x+d,o,"open_hand");
2078  pushAction(x,o);
2079  pushAction("close_hand");
2080 
2081  return true;
2082  }
2083  else
2084  return false;
2085 }
2086 
2087 
2088 /************************************************************************/
2089 bool ActionPrimitivesLayer1::touch(const Vector &x, const Vector &o, const Vector &d)
2090 {
2091  if (configured)
2092  {
2093  printMessage(log::no_info,"start touching");
2094 
2095  pushAction(x+d,o,"karate_hand");
2096  pushAction(x,o);
2097 
2098  return true;
2099  }
2100  else
2101  return false;
2102 }
2103 
2104 
2105 /************************************************************************/
2106 bool ActionPrimitivesLayer1::tap(const Vector &x1, const Vector &o1,
2107  const Vector &x2, const Vector &o2,
2108  const double execTime)
2109 {
2110  if (configured)
2111  {
2112  printMessage(log::no_info,"start tapping");
2113 
2114  pushAction(x1,o1,"karate_hand");
2115  pushAction(x2,o2,execTime);
2116  pushAction(x1,o1);
2117 
2118  return true;
2119  }
2120  else
2121  return false;
2122 }
2123 
2124 
2125 /************************************************************************/
2127 {
2128  // lift up the hand iff contact detected
2129  if (action->contactDetected)
2130  {
2131  Vector x,o;
2132  action->cartCtrl->getPose(x,o);
2133  action->printMessage(log::no_info,"logged 3-d pos: [%s]",
2134  x.toString(3,3).c_str());
2135 
2137  }
2138 
2140  action->pushAction("close_hand");
2141 }
2142 
2143 
2144 /************************************************************************/
2146 {
2147  // just disable the contact detection
2149 }
2150 
2151 
2152 /************************************************************************/
2154 {
2155  init();
2156 }
2157 
2158 
2159 /************************************************************************/
2162 {
2163  init();
2164  skipFatherPart=true;
2165  open(opt);
2166 }
2167 
2168 
2169 /************************************************************************/
2171 {
2172  skipFatherPart=false;
2173  configuredLayer2=false;
2174  contactDetectionOn=false;
2175  contactDetected=false;
2176 
2177  execLiftAndGrasp=NULL;
2178  execTouch=NULL;
2179 }
2180 
2181 
2182 /************************************************************************/
2184 {
2185  // init the contact variable
2186  contactDetected=false;
2187 
2188  // call the main postReachCallback()
2190 }
2191 
2192 
2193 /************************************************************************/
2195 {
2196  // skip until this layer is configured
2197  if (!configuredLayer2)
2198  return;
2199 
2200  // get the input from WBDYN
2201  if (Vector *wbdynWrench=wbdynPortIn.read(false))
2202  {
2203  size_t len=std::min(wbdynWrench->length(),wrenchExternal.length());
2204  for (size_t i=0; i<len; i++)
2205  wrenchExternal[i]=(*wbdynWrench)[i];
2206  }
2207 
2208  Vector forceExternal=wrenchExternal.subVector(0,2);
2209  const double forceExternalAbs=norm(forceExternal);
2210 
2211  // stop the arm iff contact detected while reaching
2212  if (!armMoveDone && contactDetectionOn && (forceExternalAbs>ext_force_thres))
2213  {
2214  if (actionWP!=NULL)
2215  {
2216  if (actionWP->isRunning())
2217  actionWP->stop();
2218 
2219  delete actionWP;
2220  actionWP=NULL;
2221  }
2222 
2223  cartCtrl->stopControl();
2224 
2225  printMessage(log::warning,"contact detected on arm: external force [%s], (%g>%g) => stopping arm",
2226  forceExternal.toString(3,3).c_str(),forceExternalAbs,ext_force_thres);
2227 
2228  disableTorsoDof();
2229 
2230  armMoveDone=true;
2231  contactDetected=true;
2232  }
2233 
2234  // call the main run()
2235  // the order does matter
2237 }
2238 
2239 
2240 /************************************************************************/
2241 bool ActionPrimitivesLayer2::open(Property &opt)
2242 {
2243  if (!skipFatherPart)
2245 
2246  if (configuredLayer2)
2247  {
2248  printMessage(log::warning,"already configured");
2249  return true;
2250  }
2251 
2252  if (configured)
2253  {
2254  ext_force_thres=opt.check("ext_force_thres",Value(std::numeric_limits<double>::max())).asFloat64();
2255  string wbdynStemName=opt.check("wbdyn_stem_name",Value(ACTIONPRIM_DEFAULT_WBDYN_STEMNAME)).asString();
2256  string wbdynPortName=opt.check("wbdyn_port_name",Value(ACTIONPRIM_DEFAULT_WBDYN_PORTNAME)).asString();
2257 
2258  // connect automatically to WTBO
2259  string wbdynServerName="/"+wbdynStemName+"/"+part+"/"+wbdynPortName;
2260  wbdynPortIn.open("/"+local+"/"+part+"/wbdyn:i");
2261  if (!Network::connect(wbdynServerName,wbdynPortIn.getName(),"udp"))
2262  {
2263  printMessage(log::error,"unable to connect to port %s",wbdynServerName.c_str());
2264 
2265  close();
2266  return false;
2267  }
2268 
2269  // create callbacks
2271  execTouch=new touchCallback(this);
2272 
2273  wrenchExternal.resize(6,0.0);
2274 
2275  return configuredLayer2=true;
2276  }
2277  else
2278  return false;
2279 }
2280 
2281 
2282 /************************************************************************/
2284 {
2286 }
2287 
2288 
2289 /************************************************************************/
2291 {
2292  if (closed)
2293  return;
2294 
2295  // call the main close()
2296  // the order does matter
2298 
2299  if (!wbdynPortIn.isClosed())
2300  {
2301  wbdynPortIn.interrupt();
2302  wbdynPortIn.close();
2303  }
2304 
2305  if (execLiftAndGrasp!=NULL)
2306  {
2307  delete execLiftAndGrasp;
2308  execLiftAndGrasp=NULL;
2309  }
2310 
2311  if (execTouch!=NULL)
2312  {
2313  delete execTouch;
2314  execTouch=NULL;
2315  }
2316 }
2317 
2318 
2319 /************************************************************************/
2320 bool ActionPrimitivesLayer2::grasp(const Vector &x, const Vector &o,
2321  const Vector &d1, const Vector &d2)
2322 {
2323  if (configured)
2324  {
2325  printMessage(log::no_info,"start grasping");
2326 
2328 
2329  pushAction(x+d1,o,"open_hand");
2331  // the remaining part is done in the callback
2332 
2333  // save data
2334  grasp_d2=d2;
2335  grasp_o=o;
2336 
2337  return true;
2338  }
2339  else
2340  return false;
2341 }
2342 
2343 
2344 /************************************************************************/
2345 bool ActionPrimitivesLayer2::grasp(const Vector &x, const Vector &o,
2346  const Vector &d)
2347 {
2348  return ActionPrimitivesLayer1::grasp(x,o,d);
2349 }
2350 
2351 
2352 /************************************************************************/
2353 bool ActionPrimitivesLayer2::touch(const Vector &x, const Vector &o, const Vector &d)
2354 {
2355  if (configured)
2356  {
2357  printMessage(log::no_info,"start touching");
2358 
2360 
2361  pushAction(x+d,o,"karate_hand");
2363 
2364  return true;
2365  }
2366  else
2367  return false;
2368 }
2369 
2370 
2371 /************************************************************************/
2372 bool ActionPrimitivesLayer2::getExtWrench(Vector &wrench) const
2373 {
2374  if (configured)
2375  {
2376  wrench=wrenchExternal;
2377  return true;
2378  }
2379  else
2380  return false;
2381 }
2382 
2383 
2384 /************************************************************************/
2386 {
2387  if (configured)
2388  {
2389  thres=ext_force_thres;
2390  return true;
2391  }
2392  else
2393  return false;
2394 }
2395 
2396 
2397 /************************************************************************/
2399 {
2400  if (configured)
2401  {
2402  ext_force_thres=thres;
2403  return true;
2404  }
2405  else
2406  return false;
2407 }
2408 
2409 
2410 /************************************************************************/
2412 {
2413  if (configured)
2414  {
2415  contactDetectionOn=true;
2416  return true;
2417  }
2418  else
2419  return false;
2420 }
2421 
2422 
2423 /************************************************************************/
2425 {
2426  if (configured)
2427  {
2428  contactDetectionOn=false;
2429  return true;
2430  }
2431  else
2432  return false;
2433 }
2434 
2435 
2436 /************************************************************************/
2438 {
2439  if (configured)
2440  {
2442  return true;
2443  }
2444  else
2445  return false;
2446 }
2447 
2448 
2449 /************************************************************************/
2451 {
2452  if (configured)
2453  {
2455  return true;
2456  }
2457  else
2458  return false;
2459 }
2460 
2461 
2462 /************************************************************************/
2464 {
2465  close();
2466 }
2467 
2468 
2469 
iCub::action::ActionPrimitives::lockActions
virtual bool lockActions()
Disable the possibility to yield any new action.
Definition: actionPrimitives.cpp:837
iCub::action::ActionPrimitives::handSeqMap
std::map< std::string, std::deque< HandWayPoint > > handSeqMap
Definition: actionPrimitives.h:298
iCub::action::ActionPrimitives::run
virtual void run()
Definition: actionPrimitives.cpp:1270
iCub::action::ActionPrimitives::postReachCallback
virtual void postReachCallback()
Definition: actionPrimitives.cpp:816
iCub::action::ArmWayPoints::ArmWayPoints
ArmWayPoints(ActionPrimitives *_action, const deque< ActionPrimitivesWayPoint > &_wayPoints)
Definition: actionPrimitives.cpp:123
iCub::action::ActionPrimitivesLayer2::touchCallback
friend class touchCallback
Definition: actionPrimitives.h:1094
iCub::action::ActionPrimitives::fingersMovingJntsSet
std::set< int > fingersMovingJntsSet
Definition: actionPrimitives.h:254
iCub
This file contains the definition of unique IDs for the body parts and the skin parts of the robot.
Definition: emotionInterface.h:17
iCub::action::ActionPrimitives::Action::execArm
bool execArm
Definition: actionPrimitives.h:275
iCub::action::ActionPrimitives::checkActionsDone
virtual bool checkActionsDone(bool &f, const bool sync=false)
Check whether all the actions in queue are accomplished.
Definition: actionPrimitives.cpp:2001
iCub::action::ActionPrimitives::polyHand
yarp::dev::PolyDriver polyHand
Definition: actionPrimitives.h:200
iCub::action::ActionPrimitives::curHandTols
yarp::sig::Vector curHandTols
Definition: actionPrimitives.h:247
iCub::action::ActionPrimitivesLayer2::disableContactDetection
virtual bool disableContactDetection()
Self-explaining :)
Definition: actionPrimitives.cpp:2424
iCub::action::ActionPrimitives::enableTorsoSw
yarp::sig::Vector enableTorsoSw
Definition: actionPrimitives.h:243
iCub::action::ActionPrimitives::locked
bool locked
Definition: actionPrimitives.h:230
iCub::action::ActionPrimitives::pushAction
virtual bool pushAction(const yarp::sig::Vector &x, const yarp::sig::Vector &o, const std::string &handSeqKey, const double execTime=ACTIONPRIM_DISABLE_EXECTIME, ActionPrimitivesCallback *clb=NULL)
Insert a combination of arm and hand primitive actions in the actions queue.
iCub::action::ActionPrimitives::_pushAction
virtual bool _pushAction(const bool execArm, const yarp::sig::Vector &x, const yarp::sig::Vector &o, const double execTime, const bool oEnabled, const bool execHand, const HandWayPoint &handWP, const bool handSeqTerminator, ActionPrimitivesCallback *clb)
iCub::action::ActionPrimitives::getCartesianIF
virtual bool getCartesianIF(yarp::dev::ICartesianControl *&ctrl) const
Return the cartesian interface used internally to control the limb.
Definition: actionPrimitives.cpp:1806
iCub::action::ActionPrimitivesLayer2::checkContact
virtual bool checkContact(bool &f) const
Check whether the reaching has been stopped due to a contact with external objects.
Definition: actionPrimitives.cpp:2450
duration
static double duration
Definition: iCub_Sim.cpp:76
iCub::action::ActionPrimitives::reachPose
virtual bool reachPose(const yarp::sig::Vector &x, const yarp::sig::Vector &o, const double execTime=ACTIONPRIM_DISABLE_EXECTIME)
Immediately update the current reaching target (without affecting the actions queue) or initiate a ne...
Definition: actionPrimitives.cpp:1147
iCub::action::ActionPrimitives::latchTimerReach
double latchTimerReach
Definition: actionPrimitives.h:237
iCub::action::ActionPrimitives::armMoveDone
bool armMoveDone
Definition: actionPrimitives.h:216
iCub::action::ActionPrimitives
Definition: actionPrimitives.h:193
iCub::action::ActionPrimitives::addHandSeqWP
virtual bool addHandSeqWP(const std::string &handSeqKey, const yarp::sig::Vector &poss, const yarp::sig::Vector &vels, const yarp::sig::Vector &tols, const yarp::sig::Vector &thres, const double tmo)
Define an hand WayPoint (WP) to be added at the bottom of the hand motion sequence pointed by the key...
Definition: actionPrimitives.cpp:1532
iCub::action::log::info
@ info
Definition: actionPrimitives.cpp:64
iCub::action::ActionPrimitives::actionsQueue
iCub::action::ActionPrimitives::ActionsQueue actionsQueue
iCub::action::ActionPrimitives::HandWayPoint::tag
std::string tag
Definition: actionPrimitives.h:261
ACTIONPRIM_TORSO_ROLL
#define ACTIONPRIM_TORSO_ROLL
Definition: actionPrimitives.cpp:51
iCub::action::ActionPrimitives::enableTorsoDof
virtual void enableTorsoDof()
Definition: actionPrimitives.cpp:1375
ACTIONPRIM_DUMP_PERIOD
#define ACTIONPRIM_DUMP_PERIOD
Definition: actionPrimitives.cpp:39
iCub::action
Definition: actionPrimitives.h:102
iCub::action::ArmWayPoints::set_default_exec_time
void set_default_exec_time(const double exec_time)
Definition: actionPrimitives.cpp:134
iCub::action::ActionPrimitives::Action::execTime
double execTime
Definition: actionPrimitives.h:278
iCub::action::ActionPrimitives::getHandSeqList
std::deque< std::string > getHandSeqList()
Return the list of available hand sequence keys.
Definition: actionPrimitives.cpp:1675
iCub::perception::SpringyFingersModel
Definition: springyFingers.h:204
iCub::action::ActionPrimitives::HandWayPoint::poss
yarp::sig::Vector poss
Definition: actionPrimitives.h:262
iCub::action::ActionPrimitivesLayer2::grasp
virtual bool grasp(const yarp::sig::Vector &x, const yarp::sig::Vector &o, const yarp::sig::Vector &d1, const yarp::sig::Vector &d2)
More evolute version of grasp.
iCub::action::ActionPrimitivesLayer2::wbdynPortIn
yarp::os::BufferedPort< yarp::sig::Vector > wbdynPortIn
Definition: actionPrimitives.h:1083
x2
x2
Definition: compute_ekf_fast.m:9
iCub::perception::Model::getOutput
virtual bool getOutput(yarp::os::Value &out) const =0
Provide the higher layers with the model output computed over the attached sensors.
ACTIONPRIM_TORSO_PITCH
#define ACTIONPRIM_TORSO_PITCH
Definition: actionPrimitives.cpp:50
iCub::action::ArmWavingMonitor::~ArmWavingMonitor
virtual ~ArmWavingMonitor()
Definition: actionPrimitives.cpp:280
iCub::action::ActionPrimitives::checkActionOnGoing
virtual bool checkActionOnGoing(bool &f, const bool sync=false)
Check whether an action is still ongoing.
Definition: actionPrimitives.cpp:2021
ACTIONPRIM_TORSO_YAW
#define ACTIONPRIM_TORSO_YAW
Definition: actionPrimitives.cpp:52
iCub::action::ActionPrimitives::Action::waitState
bool waitState
Definition: actionPrimitives.h:272
iCub::action::ActionPrimitives::fingerInPosition
std::deque< bool > fingerInPosition
Definition: actionPrimitives.h:222
iCub::action::ActionPrimitives::fingersJnts
std::vector< int > fingersJnts
Definition: actionPrimitives.h:252
iCub::action::ActionPrimitives::unlockActions
virtual bool unlockActions()
Enable the possibility to yield new actions.
Definition: actionPrimitives.cpp:850
iCub::action::ActionPrimitives::curGraspDetectionThres
yarp::sig::Vector curGraspDetectionThres
Definition: actionPrimitives.h:248
iCub::action::ActionPrimitives::HandWayPoint::tols
yarp::sig::Vector tols
Definition: actionPrimitives.h:264
out
out
Definition: sine.m:8
iCub::action::ActionPrimitives::Action::wayPointsThr
yarp::os::PeriodicThread * wayPointsThr
Definition: actionPrimitives.h:286
iCub::action::ActionPrimitives::Action::handWP
HandWayPoint handWP
Definition: actionPrimitives.h:282
iCub::action::ActionPrimitives::cv_motionDoneEvent
std::condition_variable cv_motionDoneEvent
Definition: actionPrimitives.h:214
actionPrimitives.h
iCub::action::ActionPrimitives::default_exec_time
double default_exec_time
Definition: actionPrimitives.h:233
iCub::action::ActionPrimitivesLayer2::init
virtual void init()
Definition: actionPrimitives.cpp:2170
strain::dsp::fsc::min
const FSC min
Definition: strain.h:49
iCub::action::ActionPrimitives::Action::tmo
double tmo
Definition: actionPrimitives.h:273
iCub::action::ActionPrimitives::isValidHandSeq
virtual bool isValidHandSeq(const std::string &handSeqKey)
Check whether a sequence key is defined.
Definition: actionPrimitives.cpp:1648
iCub::action::ArmWayPoints::threadInit
bool threadInit()
Definition: actionPrimitives.cpp:140
iCub::action::ActionPrimitives::HandWayPoint::vels
yarp::sig::Vector vels
Definition: actionPrimitives.h:263
iCub::action::ActionPrimitives::Action::clb
ActionPrimitivesCallback * clb
Definition: actionPrimitives.h:288
iCub::action::ActionPrimitivesLayer2::isValid
virtual bool isValid() const
Check if the object is initialized correctly.
Definition: actionPrimitives.cpp:2283
RES_WAVER
#define RES_WAVER(x)
Definition: actionPrimitives.cpp:34
iCub::action::ActionPrimitives::getHandSequence
virtual bool getHandSequence(const std::string &handSeqKey, yarp::os::Bottle &sequence)
Return a hand sequence.
Definition: actionPrimitives.cpp:1688
iCub::action::ActionPrimitivesLayer2::close
virtual void close()
Deallocate the object.
Definition: actionPrimitives.cpp:2290
ACTIONPRIM_DEFAULT_PART
#define ACTIONPRIM_DEFAULT_PART
Definition: actionPrimitives.cpp:40
stop
static int stop
Definition: iCub_Sim.cpp:41
ACTIONPRIM_DEFAULT_REACHTOL
#define ACTIONPRIM_DEFAULT_REACHTOL
Definition: actionPrimitives.cpp:38
iCub::action::ActionPrimitives::mtx
std::mutex mtx
Definition: actionPrimitives.h:210
yarp::dev
Definition: DebugInterfaces.h:52
iCub::action::ActionPrimitives::latchArmMoveDone
bool latchArmMoveDone
Definition: actionPrimitives.h:218
iCub::action::ActionPrimitives::~ActionPrimitives
virtual ~ActionPrimitives()
Destructor.
Definition: actionPrimitives.cpp:1357
iCub::action::ActionPrimitives::waitTmo
double waitTmo
Definition: actionPrimitives.h:234
iCub::action::ArmWayPoints::run
void run()
Definition: actionPrimitives.cpp:156
iCub::action::liftAndGraspCallback::exec
virtual void exec()
Defines the callback body to be called at the action end.
Definition: actionPrimitives.cpp:2126
iCub::action::ActionPrimitives::ArmWayPoints
friend class ArmWayPoints
Definition: actionPrimitives.h:257
iCub::action::ActionPrimitives::Action::handSeqTerminator
bool handSeqTerminator
Definition: actionPrimitives.h:283
iCub::action::ActionPrimitives::disableReachingTimeout
virtual bool disableReachingTimeout()
Disable timeout while reaching.
Definition: actionPrimitives.cpp:1988
state::error
@ error
math.h
iCub::action::ActionPrimitives::isHandSeqEnded
virtual bool isHandSeqEnded()
Definition: actionPrimitives.cpp:746
iCub::action::ActionPrimitives::printMessage
virtual void printMessage(const int logtype, const char *format,...) const
Definition: actionPrimitives.cpp:371
ACTIONPRIM_BALANCEARM_PERIOD
#define ACTIONPRIM_BALANCEARM_PERIOD
Definition: actionPrimitives.cpp:47
iCub::action::ActionPrimitives::reachTmoEnabled
bool reachTmoEnabled
Definition: actionPrimitives.h:229
iCub::action::ActionPrimitivesLayer2::touch
virtual bool touch(const yarp::sig::Vector &x, const yarp::sig::Vector &o, const yarp::sig::Vector &d)
More evolute version of touch, exploiting contact detection.
Definition: actionPrimitives.cpp:2353
iCub::action::ActionPrimitives::setDefaultExecTime
virtual bool setDefaultExecTime(const double execTime)
Set the default arm movement execution time.
Definition: actionPrimitives.cpp:1894
iCub::action::ActionPrimitives::disableArmWaving
virtual bool disableArmWaving()
Disable the waving mode.
Definition: actionPrimitives.cpp:1958
iCub::action::ActionPrimitivesLayer2::getExtWrench
virtual bool getExtWrench(yarp::sig::Vector &wrench) const
Retrieve the current wrench on the end-effector.
Definition: actionPrimitives.cpp:2372
iCub::action::ArmWavingMonitor::enable
bool enable()
Definition: actionPrimitives.cpp:234
ACTIONPRIM_DEFAULT_PER
#define ACTIONPRIM_DEFAULT_PER
Definition: actionPrimitives.cpp:36
iCub::action::ActionPrimitivesLayer2::grasp_o
yarp::sig::Vector grasp_o
Definition: actionPrimitives.h:1088
iCub::action::ActionPrimitivesLayer2::liftAndGraspCallback
friend class liftAndGraspCallback
Definition: actionPrimitives.h:1093
strain::dsp::fsc::max
const FSC max
Definition: strain.h:48
iCub::action::ActionPrimitives::Action::execWayPoints
bool execWayPoints
Definition: actionPrimitives.h:285
iCub::action::ActionPrimitivesLayer2::configuredLayer2
bool configuredLayer2
Definition: actionPrimitives.h:1074
iCub::action::ActionPrimitives::polyCart
yarp::dev::PolyDriver polyCart
Definition: actionPrimitives.h:201
iCub::action::ActionPrimitives::actionClb
ActionPrimitivesCallback * actionClb
Definition: actionPrimitives.h:291
iCub::action::ActionPrimitives::latchTimerWait
double latchTimerWait
Definition: actionPrimitives.h:236
iCub::action::ActionPrimitivesLayer1
Definition: actionPrimitives.h:931
iCub::action::ArmWavingMonitor::ArmWavingMonitor
ArmWavingMonitor(ICartesianControl *_cartCtrl)
Definition: actionPrimitives.cpp:211
iCub::action::ActionPrimitives::configGraspModel
virtual bool configGraspModel(yarp::os::Property &opt)
Definition: actionPrimitives.cpp:529
iCub::action::ActionPrimitives::jHandMin
int jHandMin
Definition: actionPrimitives.h:240
iCub::action::ArmWayPoints
Definition: actionPrimitives.cpp:75
iCub::action::ActionPrimitivesLayer2::execLiftAndGrasp
liftAndGraspCallback * execLiftAndGrasp
Definition: actionPrimitives.h:1080
iCub::action::ActionPrimitives::jHandMax
int jHandMax
Definition: actionPrimitives.h:241
iCub::action::ActionPrimitivesLayer2::~ActionPrimitivesLayer2
virtual ~ActionPrimitivesLayer2()
Destructor.
Definition: actionPrimitives.cpp:2463
iCub::action::ActionPrimitives::Action::x
yarp::sig::Vector x
Definition: actionPrimitives.h:276
iCub::action::ActionPrimitives::getGraspModel
virtual bool getGraspModel(perception::Model *&model) const
Return the model used internally to detect external contacts.
Definition: actionPrimitives.cpp:1793
iCub::action::ActionPrimitivesCallback::exec
virtual void exec()=0
Defines the callback body to be called at the action end.
iCub::action::ActionPrimitives::encCtrl
yarp::dev::IEncoders * encCtrl
Definition: actionPrimitives.h:203
iCub::action::ActionPrimitives::latchTimerHand
double latchTimerHand
Definition: actionPrimitives.h:250
iCub::action::ActionPrimitives::setTrackingMode
virtual bool setTrackingMode(const bool f)
Set the task space controller in tracking or non-tracking mode.
Definition: actionPrimitives.cpp:1914
iCub::action::ArmWavingMonitor::afterStart
void afterStart(bool success)
Definition: actionPrimitives.cpp:225
iCub::action::ActionPrimitives::areFingersInPosition
virtual bool areFingersInPosition(bool &f)
Query if fingers are in position (cumulative response).
Definition: actionPrimitives.cpp:1765
iCub::action::log::no_info
@ no_info
Definition: actionPrimitives.cpp:64
iCub::action::ActionPrimitives::closed
bool closed
Definition: actionPrimitives.h:225
iCub::action::ActionPrimitives::Action::oEnabled
bool oEnabled
Definition: actionPrimitives.h:279
iCub::action::ActionPrimitives::ActionPrimitives
ActionPrimitives()
Default Constructor.
Definition: actionPrimitives.cpp:321
iCub::action::ActionPrimitives::isValid
virtual bool isValid() const
Check if the object is initialized correctly.
Definition: actionPrimitives.cpp:364
iCub::action::ActionPrimitivesLayer2::enableContactDetection
virtual bool enableContactDetection()
Self-explaining :)
Definition: actionPrimitives.cpp:2411
iCub::action::ActionPrimitives::getPose
virtual bool getPose(yarp::sig::Vector &x, yarp::sig::Vector &o) const
Get the current arm pose.
Definition: actionPrimitives.cpp:1848
iCub::action::ActionPrimitives::handMoveDone
bool handMoveDone
Definition: actionPrimitives.h:217
iCub::ctrl
Definition: adaptWinPolyEstimator.h:37
iCub::action::ActionPrimitives::Action::execHand
bool execHand
Definition: actionPrimitives.h:281
ACTIONPRIM_DISABLE_EXECTIME
#define ACTIONPRIM_DISABLE_EXECTIME
Definition: actionPrimitives.h:96
iCub::action::ActionPrimitives::handleTorsoDOF
virtual bool handleTorsoDOF(yarp::os::Property &opt, const std::string &key)
Definition: actionPrimitives.cpp:404
ACTIONPRIM_DEFAULT_WBDYN_STEMNAME
#define ACTIONPRIM_DEFAULT_WBDYN_STEMNAME
Definition: actionPrimitives.cpp:43
iCub::action::ActionPrimitives::open
virtual bool open(yarp::os::Property &opt)
Configure the object.
Definition: actionPrimitives.cpp:582
iCub::action::ActionPrimitivesLayer1::grasp
virtual bool grasp(const yarp::sig::Vector &x, const yarp::sig::Vector &o, const yarp::sig::Vector &d)
Grasp the given target (combined action).
Definition: actionPrimitives.cpp:2071
iCub::action::ActionPrimitives::getDefaultExecTime
virtual double getDefaultExecTime() const
Get the current default arm movement execution time.
Definition: actionPrimitives.cpp:1907
ACTIONPRIM_DEFAULT_VERBOSITY
#define ACTIONPRIM_DEFAULT_VERBOSITY
Definition: actionPrimitives.cpp:42
iCub::ctrl::norm
double norm(const yarp::sig::Matrix &M, int col)
iCub::action::ActionPrimitives::cmdArmWP
virtual bool cmdArmWP(const Action &action)
Definition: actionPrimitives.cpp:1466
iCub::perception::TactileFingersModel
Definition: tactileFingers.h:151
x
x
Definition: compute_ekf_sym.m:21
iCub::action::ActionPrimitives::graspModel
perception::Model * graspModel
Definition: actionPrimitives.h:207
iCub::action::ActionPrimitives::cmdHand
virtual bool cmdHand(const Action &action)
Definition: actionPrimitives.cpp:1484
iCub::action::log::error
@ error
Definition: actionPrimitives.cpp:64
iCub::action::ActionPrimitives::syncCheckReinstate
virtual bool syncCheckReinstate()
Reinstate the blocking feature for future calls with sync switch on.
Definition: actionPrimitives.cpp:2058
iCub::action::ActionPrimitivesLayer2::postReachCallback
virtual void postReachCallback()
Definition: actionPrimitives.cpp:2183
iCub::action::ActionPrimitives::cv_motionStartEvent
std::condition_variable cv_motionStartEvent
Definition: actionPrimitives.h:212
iCub::action::ActionPrimitives::part
std::string part
Definition: actionPrimitives.h:198
iCub::action::ActionPrimitives::HandWayPoint::thres
yarp::sig::Vector thres
Definition: actionPrimitives.h:265
iCub::action::ActionPrimitives::syncCheckInterrupt
virtual bool syncCheckInterrupt(const bool disable=false)
Suddenly interrupt any blocking call that is pending on querying the action status.
Definition: actionPrimitives.cpp:2041
iCub::action::ActionPrimitives::reachPosition
virtual bool reachPosition(const yarp::sig::Vector &x, const double execTime=ACTIONPRIM_DISABLE_EXECTIME)
Immediately update the current reaching target (without affecting the actions queue) or initiate a ne...
Definition: actionPrimitives.cpp:1176
iCub::action::ActionPrimitives::fingersJntsSet
std::set< int > fingersJntsSet
Definition: actionPrimitives.h:253
iCub::action::ActionPrimitives::HandWayPoint::tmo
double tmo
Definition: actionPrimitives.h:266
iCub::action::ActionPrimitives::init
virtual void init()
Definition: actionPrimitives.cpp:338
iCub::action::ActionPrimitives::configured
bool configured
Definition: actionPrimitives.h:224
iCub::action::ActionPrimitives::local
std::string local
Definition: actionPrimitives.h:197
f
f
Definition: compute_ekf_sym.m:22
iCub::action::ActionPrimitives::checkEnabled
bool checkEnabled
Definition: actionPrimitives.h:226
iCub::action::liftAndGraspCallback::action
ActionPrimitivesLayer2 * action
Definition: actionPrimitives.h:1036
iCub::action::ActionPrimitives::close
virtual void close()
Deallocate the object.
Definition: actionPrimitives.cpp:701
iCub::action::ActionPrimitives::latchTimerReachLog
double latchTimerReachLog
Definition: actionPrimitives.h:238
iCub::action::ActionPrimitives::curHandTmo
double curHandTmo
Definition: actionPrimitives.h:249
iCub::action::ActionPrimitives::enableReachingTimeout
virtual bool enableReachingTimeout(const double tmo)
Enable timeout while reaching.
Definition: actionPrimitives.cpp:1973
ACTIONPRIM_DEFAULT_EXECTIME
#define ACTIONPRIM_DEFAULT_EXECTIME
Definition: actionPrimitives.cpp:37
iCub::action::ActionPrimitivesLayer2::execTouch
touchCallback * execTouch
Definition: actionPrimitives.h:1081
iCub::action::ActionPrimitivesLayer2::run
virtual void run()
Definition: actionPrimitives.cpp:2194
iCub::action::ArmWavingMonitor::disable
bool disable()
Definition: actionPrimitives.cpp:254
iCub::action::touchCallback::exec
virtual void exec()
Defines the callback body to be called at the action end.
Definition: actionPrimitives.cpp:2145
iCub::action::ActionPrimitivesLayer2::contactDetectionOn
bool contactDetectionOn
Definition: actionPrimitives.h:1075
iCub::action::ActionPrimitives::posCtrl
yarp::dev::IPositionControl * posCtrl
Definition: actionPrimitives.h:204
ACTIONPRIM_DEFAULT_TRACKINGMODE
#define ACTIONPRIM_DEFAULT_TRACKINGMODE
Definition: actionPrimitives.cpp:41
iCub::action::ActionPrimitives::removeHandSeq
virtual bool removeHandSeq(const std::string &handSeqKey)
Remove an already existing hand motion sequence.
Definition: actionPrimitives.cpp:1660
iCub::action::ActionPrimitives::setTorsoJoints
virtual bool setTorsoJoints(const yarp::sig::Vector &torso)
Change the control status of torso joints.
Definition: actionPrimitives.cpp:1832
tactileFingers.h
iCub::action::ActionPrimitives::ActionsQueue::clear
void clear()
Definition: actionPrimitives.cpp:307
iCub::action::ActionPrimitives::pushWaitState
virtual bool pushWaitState(const double tmo, ActionPrimitivesCallback *clb=NULL)
Insert a wait state in the actions queue.
Definition: actionPrimitives.cpp:1123
iCub::action::ActionPrimitives::mtx_motionStartEvent
std::mutex mtx_motionStartEvent
Definition: actionPrimitives.h:211
x1
x1
Definition: compute_ekf_sym.m:28
iCub::action::ActionPrimitives::tracking_mode
bool tracking_mode
Definition: actionPrimitives.h:227
iCub::perception::Model
Definition: models.h:61
iCub::action::ActionPrimitives::Action
Definition: actionPrimitives.h:269
iCub::action::ActionPrimitives::areFingersMoving
virtual bool areFingersMoving(bool &f)
Query if fingers are moving.
Definition: actionPrimitives.cpp:1751
iCub::perception::Model::fromProperty
virtual bool fromProperty(const yarp::os::Property &options)=0
Configure the model taking its parameters from a Property object.
iCub::action::ActionPrimitives::reachTmo
double reachTmo
Definition: actionPrimitives.h:235
iCub::action::ArmWavingMonitor::setRestPosition
void setRestPosition(const Vector &_restPos)
Definition: actionPrimitives.cpp:219
iCub::action::ActionPrimitives::curHandFinalPoss
yarp::sig::Vector curHandFinalPoss
Definition: actionPrimitives.h:246
ACTIONPRIM_BALANCEARM_LENGTH
#define ACTIONPRIM_BALANCEARM_LENGTH
Definition: actionPrimitives.cpp:48
iCub::action::ActionPrimitivesLayer2::isContactDetectionEnabled
virtual bool isContactDetectionEnabled(bool &f) const
Self-explaining :)
Definition: actionPrimitives.cpp:2437
iCub::action::ActionPrimitives::execQueuedAction
virtual bool execQueuedAction()
Definition: actionPrimitives.cpp:1204
iCub::action::ActionPrimitives::disableTorsoDof
virtual void disableTorsoDof()
Definition: actionPrimitives.cpp:1388
iCub::action::ActionPrimitives::actionWP
yarp::os::PeriodicThread * actionWP
Definition: actionPrimitives.h:292
ACTIONPRIM_DEFAULT_WBDYN_PORTNAME
#define ACTIONPRIM_DEFAULT_WBDYN_PORTNAME
Definition: actionPrimitives.cpp:44
iCub::action::ActionPrimitives::armWaver
yarp::os::PeriodicThread * armWaver
Definition: actionPrimitives.h:209
iCub::action::ActionPrimitives::configHandSeq
virtual bool configHandSeq(yarp::os::Property &opt)
Definition: actionPrimitives.cpp:465
iCub::action::ActionPrimitives::robot
std::string robot
Definition: actionPrimitives.h:196
iCub::action::ActionPrimitivesCallback
Definition: actionPrimitives.h:111
iCub::action::ActionPrimitives::verbose
bool verbose
Definition: actionPrimitives.h:231
iCub::action::ActionPrimitivesLayer2::ActionPrimitivesLayer2
ActionPrimitivesLayer2()
Default Constructor.
Definition: actionPrimitives.cpp:2153
iCub::action::ActionPrimitives::stopControl
virtual bool stopControl()
Stop any ongoing arm/hand movements.
Definition: actionPrimitives.cpp:1861
iCub::action::ActionPrimitivesLayer2::grasp_d2
yarp::sig::Vector grasp_d2
Definition: actionPrimitives.h:1087
iCub::action::ActionPrimitives::cartCtrl
yarp::dev::ICartesianControl * cartCtrl
Definition: actionPrimitives.h:205
iCub::action::ActionPrimitivesLayer2::open
virtual bool open(yarp::os::Property &opt)
Configure the object.
Definition: actionPrimitives.cpp:2241
iCub::action::ActionPrimitivesLayer2::setExtForceThres
virtual bool setExtForceThres(const double thres)
Set the threshold on the external force used to stop the limb while reaching.
Definition: actionPrimitives.cpp:2398
springyFingers.h
iCub::action::touchCallback::action
ActionPrimitivesLayer2 * action
Definition: actionPrimitives.h:1050
iCub::action::ActionPrimitives::handCheckMotionDone
virtual bool handCheckMotionDone(const int jnt)
Definition: actionPrimitives.cpp:1364
iCub::action::ActionPrimitives::fingers2JntsMap
std::multimap< int, int > fingers2JntsMap
Definition: actionPrimitives.h:255
iCub::action::ActionPrimitives::torsoActive
bool torsoActive
Definition: actionPrimitives.h:228
iCub::action::ActionPrimitivesLayer1::tap
virtual bool tap(const yarp::sig::Vector &x1, const yarp::sig::Vector &o1, const yarp::sig::Vector &x2, const yarp::sig::Vector &o2, const double execTime=ACTIONPRIM_DISABLE_EXECTIME)
Tap the given target (combined action).
Definition: actionPrimitives.cpp:2106
iCub::action::ArmWavingMonitor::run
void run()
Definition: actionPrimitives.cpp:267
iCub::action::ActionPrimitives::enableArmWaving
virtual bool enableArmWaving(const yarp::sig::Vector &restPos)
Enable the waving mode that keeps on moving the arm around a predefined position.
Definition: actionPrimitives.cpp:1939
iCub::action::ActionPrimitives::clearActionsQueue
virtual bool clearActionsQueue()
Empty the actions queue.
Definition: actionPrimitives.cpp:823
iCub::action::ActionPrimitives::wait
virtual bool wait(const Action &action)
Definition: actionPrimitives.cpp:1401
iCub::action::ActionPrimitives::cmdArm
virtual bool cmdArm(const Action &action)
Definition: actionPrimitives.cpp:1417
iCub::action::ActionPrimitives::addHandSequence
virtual bool addHandSequence(const std::string &handSeqKey, const yarp::os::Bottle &sequence)
Define an hand motion sequence from a configuration bottle.
Definition: actionPrimitives.cpp:1557
iCub::action::ActionPrimitives::disableTorsoSw
yarp::sig::Vector disableTorsoSw
Definition: actionPrimitives.h:244
iCub::action::log::warning
@ warning
Definition: actionPrimitives.cpp:64
iCub::action::ActionPrimitives::handSeqTerminator
bool handSeqTerminator
Definition: actionPrimitives.h:220
iCub::action::ActionPrimitivesLayer2::contactDetected
bool contactDetected
Definition: actionPrimitives.h:1076
iCub::action::ArmWayPoints::~ArmWayPoints
virtual ~ArmWayPoints()
Definition: actionPrimitives.cpp:193
time
degrees time
Definition: sine.m:5
iCub::action::ActionPrimitives::mtx_motionDoneEvent
std::mutex mtx_motionDoneEvent
Definition: actionPrimitives.h:213
iCub::action::ActionPrimitivesLayer2::getExtForceThres
virtual bool getExtForceThres(double &thres) const
Retrieve the current threshold on the external force used to stop the limb while reaching.
Definition: actionPrimitives.cpp:2385
iCub::action::ActionPrimitives::modCtrl
yarp::dev::IControlMode * modCtrl
Definition: actionPrimitives.h:202
iCub::action::ActionPrimitives::HandWayPoint
Definition: actionPrimitives.h:259
iCub::action::ActionPrimitivesLayer2::skipFatherPart
bool skipFatherPart
Definition: actionPrimitives.h:1073
iCub::action::ActionPrimitives::getTrackingMode
virtual bool getTrackingMode() const
Get the current controller mode.
Definition: actionPrimitives.cpp:1932
iCub::action::ActionPrimitives::execPendingHandSequences
virtual bool execPendingHandSequences()
Definition: actionPrimitives.cpp:1245
iCub::action::ActionPrimitives::fingersInPosition
bool fingersInPosition
Definition: actionPrimitives.h:221
iCub::action::ActionPrimitives::Action::o
yarp::sig::Vector o
Definition: actionPrimitives.h:277
iCub::action::ActionPrimitivesLayer2::wrenchExternal
yarp::sig::Vector wrenchExternal
Definition: actionPrimitives.h:1085
iCub::action::ActionPrimitivesLayer1::touch
virtual bool touch(const yarp::sig::Vector &x, const yarp::sig::Vector &o, const yarp::sig::Vector &d)
Touch the given target (combined action).
Definition: actionPrimitives.cpp:2089
iCub::action::ActionPrimitivesLayer2::ext_force_thres
double ext_force_thres
Definition: actionPrimitives.h:1078
iCub::perception
Definition: models.h:52
iCub::action::ArmWavingMonitor
Definition: actionPrimitives.cpp:203
iCub::action::ActionPrimitives::getActionsLockStatus
virtual bool getActionsLockStatus() const
Return the actions lock status.
Definition: actionPrimitives.cpp:863
iCub::action::ActionPrimitives::getTorsoJoints
virtual bool getTorsoJoints(yarp::sig::Vector &torso)
Return the control status of torso joints.
Definition: actionPrimitives.cpp:1819
iCub::action::ActionPrimitives::latchHandMoveDone
bool latchHandMoveDone
Definition: actionPrimitives.h:219