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