iCub-main
MotorThread.cpp
Go to the documentation of this file.
1 /*
2 * Copyright (C) 2010 RobotCub Consortium, European Commission FP6 Project IST-004370
3 * Author: Carlo Ciliberto, Vadim Tikhanoff
4 * email: carlo.ciliberto@iit.it vadim.tikhanoff@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 
20 #include <fstream>
21 #include <sstream>
22 #include <cmath>
23 #include <vector>
24 #include <algorithm>
25 #include <iomanip>
26 
27 #include <yarp/os/Time.h>
28 #include <yarp/math/Math.h>
29 #include <yarp/math/Rand.h>
30 
31 #include <iCub/ctrl/math.h>
32 #include <iCub/iKin/iKinFwd.h>
33 
34 #include <iCub/MotorThread.h>
35 #include <iCub/pointing_far.h>
36 
37 using namespace iCub::ctrl;
38 using namespace iCub::iKin;
39 using namespace iCub::perception;
40 
41 
42 bool MotorThread::checkOptions(const Bottle &options, const string &parameter)
43 {
44  bool found=false;
45  for (int i=0; i<options.size(); i++)
46  {
47  if(options.get(i).asString()==parameter)
48  {
49  found=true;
50  break;
51  }
52  }
53 
54  return found;
55 }
56 
57 
58 void Dragger::init(Bottle &initializer, double thread_rate)
59 {
60  extForceThresh=initializer.check("external_force_thresh",Value(1e9)).asFloat64();
61  samplingRate=initializer.check("subsampling_rate",Value(500.0)).asFloat64();
62 
63  damping=initializer.check("damping",Value(1e9)).asFloat64();
64  inertia=initializer.check("inertia",Value(1e9)).asFloat64();
65 
66  Vector zeros3d(3);
67  zeros3d=0.0;
68  I=new Integrator(thread_rate/1000.0,zeros3d);
69 
70  ctrl=NULL;
71  actionName="";
72 }
73 
74 
75 bool MotorThread::storeContext(const int arm)
76 {
77  if ((arm==LEFT) || (arm==RIGHT))
78  {
79  if (action[arm]!=NULL)
80  {
81  ICartesianControl *ctrl=NULL;
82  if (action[arm]->getCartesianIF(ctrl))
83  {
84  if (ctrl!=NULL)
85  {
86  ctrl->storeContext(&action_context[arm]);
87  return true;
88  }
89  }
90  }
91  }
92 
93  return false;
94 }
95 
96 
97 bool MotorThread::restoreContext(const int arm)
98 {
99  if ((arm==LEFT) || (arm==RIGHT))
100  {
101  if (action[arm]!=NULL)
102  {
103  ICartesianControl *ctrl=NULL;
104  if (action[arm]->getCartesianIF(ctrl))
105  {
106  if (ctrl!=NULL)
107  {
108  ctrl->stopControl();
109  ctrl->restoreContext(action_context[arm]);
110  return true;
111  }
112  }
113  }
114  }
115 
116  return false;
117 }
118 
119 
120 bool MotorThread::deleteContext(const int arm)
121 {
122  if ((arm==LEFT) || (arm==RIGHT))
123  {
124  if (action[arm]!=NULL)
125  {
126  ICartesianControl *ctrl=NULL;
127  if (action[arm]->getCartesianIF(ctrl))
128  {
129  if (ctrl!=NULL)
130  {
131  ctrl->deleteContext(action_context[arm]);
132  return true;
133  }
134  }
135  }
136  }
137 
138  return false;
139 }
140 
141 
142 bool MotorThread::storeContext(ActionPrimitives *action)
143 {
144  if (action==&action[LEFT])
145  return storeContext(LEFT);
146  else if (action==&action[RIGHT])
147  return storeContext(RIGHT);
148  else
149  return false;
150 }
151 
152 
153 bool MotorThread::restoreContext(ActionPrimitives *action)
154 {
155  if (action==&action[LEFT])
156  return restoreContext(LEFT);
157  else if (action==&action[RIGHT])
158  return restoreContext(RIGHT);
159  else
160  return false;
161 }
162 
163 
164 bool MotorThread::deleteContext(ActionPrimitives *action)
165 {
166  if (action==&action[LEFT])
167  return deleteContext(LEFT);
168  else if (action==&action[RIGHT])
169  return deleteContext(RIGHT);
170  else
171  return false;
172 }
173 
174 
175 int MotorThread::checkArm(int arm)
176 {
177  if(arm!=ARM_IN_USE && arm!=ARM_MOST_SUITED && action[arm]!=NULL)
178  armInUse=arm;
179 
180  return armInUse;
181 }
182 
183 
184 int MotorThread::checkArm(int arm, Vector &xd, const bool applyOffset)
185 {
186  if(arm==ARM_MOST_SUITED)
187  arm=xd[1]<0.0?LEFT:RIGHT;
188 
189  if(arm!=ARM_IN_USE && arm!=ARM_MOST_SUITED && action[arm]!=NULL)
190  armInUse=arm;
191 
192  if (applyOffset)
193  {
194  xd[0]+=currentKinematicOffset[armInUse][0];
195  xd[1]+=currentKinematicOffset[armInUse][1];
196  xd[2]+=currentKinematicOffset[armInUse][2];
197  }
198 
199  return checkArm(arm);
200 }
201 
202 
203 bool MotorThread::setImpedance(bool turn_on)
204 {
205  InteractionModeEnum mode=(turn_on?VOCAB_IM_COMPLIANT:VOCAB_IM_STIFF);
206  for(int i=0; i<5; i++)
207  {
208  if(action[LEFT]!=NULL)
209  int_mode_arm[LEFT]->setInteractionMode(i,mode);
210  if(action[RIGHT]!=NULL)
211  int_mode_arm[RIGHT]->setInteractionMode(i,mode);
212  }
213 
214  if (int_mode_torso!=NULL)
215  {
216  InteractionModeEnum modes[3]={VOCAB_IM_STIFF, VOCAB_IM_STIFF, VOCAB_IM_STIFF};
217  int_mode_torso->setInteractionModes(modes);
218  }
219 
220  status_impedance_on=turn_on;
221  return true;
222 }
223 
224 
225 bool MotorThread::setTorque(bool turn_on, int arm)
226 {
227  if(action[arm]==NULL)
228  return false;
229 
230  bool done=true;
231 
232  //if the system is asked to turn on impedance control
233  if(turn_on)
234  {
235  for(int i=0; i<4; i++)
236  done=done && ctrl_mode_arm[arm]->setControlMode(i,VOCAB_CM_TORQUE);
237 
238  for(int i=0; i<3; i++)
239  done=done && int_mode_torso->setInteractionMode(i,VOCAB_IM_COMPLIANT);
240  }
241  else
242  done=setImpedance(status_impedance_on);
243 
244  return done;
245 }
246 
247 
248 int MotorThread::setStereoToCartesianMode(const int mode)
249 {
250  Bottle reply;
251  return setStereoToCartesianMode(mode,reply);
252 }
253 
254 
255 int MotorThread::setStereoToCartesianMode(const int mode, Bottle &reply)
256 {
257  switch(mode)
258  {
259  case S2C_DISPARITY:
260  {
261  if (Network::isConnected(disparityPort.getName(),"/stereoDisparity/rpc"))
262  {
263  modeS2C=S2C_DISPARITY;
264  string tmp="[Stereo -> Cartesian]: Disparity";
265  yInfo("%s",tmp.c_str());
266  reply.addString(tmp);
267  }
268  else
269  {
270  modeS2C=S2C_HOMOGRAPHY;
271  string tmp="[Stereo -> Cartesian]: Homography";
272  yInfo("%s",tmp.c_str());
273  reply.addString(tmp);
274  }
275  break;
276  }
277 
278  case S2C_NETWORK:
279  {
280  if (neuralNetworkAvailable)
281  {
282  modeS2C=S2C_NETWORK;
283  string tmp="[Stereo -> Cartesian]: Neural Net";
284  yInfo("%s",tmp.c_str());
285  reply.addString(tmp);
286  }
287  else
288  {
289  modeS2C=S2C_HOMOGRAPHY;
290  string tmp="[Stereo -> Cartesian]: Homography";
291  yInfo("%s",tmp.c_str());
292  reply.addString(tmp);
293  }
294  break;
295  }
296 
297  default:
298  {
299  modeS2C=S2C_HOMOGRAPHY;
300  string tmp="[Stereo -> Cartesian]: Homography";
301  yInfo("%s",tmp.c_str());
302  reply.addString(tmp);
303  break;
304  }
305  }
306 
307  //get left offsets
308  if(!bKinOffsets.find("left").asList()->check(Vocab32::decode(modeS2C)))
309  {
310  Bottle &bKinMode=bKinOffsets.find("left").asList()->addList();
311  bKinMode.addString(Vocab32::decode(modeS2C));
312  Bottle &bKinVec=bKinMode.addList();
313  for(int i=0; i<3; i++)
314  bKinVec.addFloat64(0.0);
315  }
316 
317  Bottle *bKinLeft=bKinOffsets.find("left").asList()->find(Vocab32::decode(modeS2C)).asList();
318  for(int i=0; i<3; i++)
319  defaultKinematicOffset[LEFT][i]=bKinLeft->get(i).asFloat64();
320 
321  //get left offsets
322  if(!bKinOffsets.find("right").asList()->check(Vocab32::decode(modeS2C)))
323  {
324  Bottle &bKinMode=bKinOffsets.find("right").asList()->addList();
325  bKinMode.addString(Vocab32::decode(modeS2C));
326  Bottle &bKinVec=bKinMode.addList();
327  for(int i=0; i<3; i++)
328  bKinVec.addFloat64(0.0);
329  }
330 
331  Bottle *bKinRight=bKinOffsets.find("right").asList()->find(Vocab32::decode(modeS2C)).asList();
332  for(int i=0; i<3; i++)
333  defaultKinematicOffset[RIGHT][i]=bKinRight->get(i).asFloat64();
334 
335  return modeS2C;
336 }
337 
338 
339 bool MotorThread::targetToCartesian(Bottle *bTarget, Vector &xd)
340 {
341  bool found=false;
342 
343  // set the default current kinematic offsets for the arms
344  currentKinematicOffset[LEFT]=defaultKinematicOffset[LEFT];
345  currentKinematicOffset[RIGHT]=defaultKinematicOffset[RIGHT];
346 
347  // if the tartget's cartesian coordinates was specified, use them.
348  if(!found && bTarget!=NULL && bTarget->check("cartesian") && bTarget->find("cartesian").asList()->size()>=3)
349  {
350  Bottle *bCartesian=bTarget->find("cartesian").asList();
351  xd.clear();
352  for(int i=0; i<bCartesian->size(); i++)
353  xd.push_back(bCartesian->get(i).asFloat64());
354  found=true;
355  }
356 
357  // if an object was specified check for its 3D position associated to the object
358  if(!found && bTarget!=NULL && bTarget->check("name"))
359  if(opcPort.getCartesianPosition(bTarget->find("name").asString(),xd))
360  found=true;
361 
362  if(!found && bTarget!=NULL && bTarget->check("stereo"))
363  {
364  Bottle *bStereo=bTarget->find("stereo").asList();
365 
366  if(bStereo!=NULL)
367  {
368  Vector stereo;
369  for(int i=0; i<bStereo->size(); i++)
370  stereo.push_back(bStereo->get(i).asFloat64());
371 
372  found=stereoToCartesian(stereo,xd);
373  }
374  }
375 
376  if(found && bTarget!=NULL && bTarget->check("name"))
377  opcPort.getKinematicOffsets(bTarget->find("name").asString(),currentKinematicOffset);
378 
379  return found;
380 }
381 
382 
383 bool MotorThread::stereoToCartesian(const Vector &stereo, Vector &xd)
384 {
385  if(stereo.size()!=4)
386  return false;
387 
388  if(arm_mode!=ARM_MODE_IDLE)
389  {
390  yWarning("System is busy!");
391  return false;
392  }
393 
394  bool ok=false;
395 
396  switch(modeS2C)
397  {
398  case S2C_HOMOGRAPHY:
399  {
400  ok=stereoToCartesianHomography(stereo,xd);
401  break;
402  }
403 
404  case S2C_DISPARITY:
405  {
406  ok=stereoToCartesianDisparity(stereo,xd);
407  break;
408  }
409 
410  case S2C_NETWORK:
411  {
412  ok=stereoToCartesianNetwork(stereo,xd);
413  break;
414  }
415  }
416 
417  return ok;
418 }
419 
420 
421 bool MotorThread::loadExplorationPoses(const string &file_name)
422 {
423  Property optExpl;
424  optExpl.fromConfigFile(rf.findFile(file_name));
425 
426  if(!optExpl.check("torso") ||!optExpl.check("hand") ||!optExpl.check("head"))
427  return false;
428 
429  Bottle &tmpTorso=optExpl.findGroup("torso");
430  for(int i=1; i<tmpTorso.size(); i++)
431  {
432  Bottle *b=tmpTorso.get(i).asList();
433  Vector v(b->size());
434  for(int j=0; j<b->size(); j++)
435  v[j]=b->get(j).asFloat64();
436  pos_torsoes.push_back(v);
437  }
438 
439  Bottle &tmpHand=optExpl.findGroup("hand");
440  for(int i=1; i<tmpHand.size(); i++)
441  {
442  Bottle *b=tmpHand.get(i).asList();
443  Vector v(b->size());
444  for(int j=0; j<b->size(); j++)
445  v[j]=b->get(j).asFloat64();
446  handPoses.push_back(v);
447  }
448 
449  Bottle &tmpHead=optExpl.findGroup("head");
450  for(int i=1; i<tmpHead.size(); i++)
451  {
452  Bottle *b=tmpHead.get(i).asList();
453  Vector v(b->size());
454  for(int j=0; j<b->size(); j++)
455  v[j]=b->get(j).asFloat64();
456  headPoses.push_back(v);
457  }
458 
459  return true;
460 }
461 
462 
463 Vector MotorThread::eye2root(const Vector &out, bool forehead)
464 {
465  Vector xd;
466  if(out.size()!=3)
467  return xd;
468 
469  Vector out_hom(4);
470  out_hom[0]=out[0];
471  out_hom[1]=out[1];
472  out_hom[2]=out[2];
473  out_hom[3]=1.0;
474 
475  Vector eyePos,eyeOrient;
476  if(forehead)
477  ctrl_gaze->getHeadPose(eyePos,eyeOrient);
478  else
479  ctrl_gaze->getLeftEyePose(eyePos,eyeOrient);
480 
481  Matrix T=axis2dcm(eyeOrient);
482  T.setSubcol(eyePos,0,3);
483 
484  Vector root=T*out_hom;
485 
486  // safe thresholding
487  if (root[0]>-0.15)
488  root[0]=-0.15;
489 
490  if (root[2]<=table_height)
491  root[2]=table_height;
492 
493  xd.resize(3);
494  xd[0]=root[0];
495  xd[1]=root[1];
496  xd[2]=root[2];
497 
498  return xd;
499 }
500 
501 
502 bool MotorThread::stereoToCartesianHomography(const Vector &stereo, Vector &xd)
503 {
504  int eye_in_use;
505  if(stereo[2*dominant_eye]==0.0 && stereo[2*dominant_eye+1]==0.0)
506  {
507  if(stereo[2*(1-dominant_eye)]==0.0 && stereo[2*(1-dominant_eye)+1]==0.0)
508  return false;
509 
510  eye_in_use=1-dominant_eye;
511  }
512  else
513  eye_in_use=dominant_eye;
514 
515  Vector px(2);
516  px[0]=stereo[2*eye_in_use];
517  px[1]=stereo[2*eye_in_use+1];
518 
519  ctrl_gaze->get3DPointOnPlane(eye_in_use,px,table,xd);
520 
521  return true;
522 }
523 
524 
525 bool MotorThread::stereoToCartesianDisparity(const Vector &stereo, Vector &xd)
526 {
527  Bottle bEye;
528  bEye.addFloat64(stereo[0]);
529  bEye.addFloat64(stereo[1]);
530 
531  Bottle bX;
532  do
533  {
534  disparityPort.write(bEye,bX);
535  }
536  while(bX.size()==0 || bX.get(2).asFloat64()<0.0 );
537 
538  if(bX.size()!=0 && bX.get(2).asFloat64()>0.0 )
539  {
540  xd.resize(3);
541  xd[0]=bX.get(0).asFloat64();
542  xd[1]=bX.get(1).asFloat64();
543  xd[2]=bX.get(2).asFloat64();
544  }
545 
546  xd=eye2root(xd,false);
547 
548  return xd.size()==3;
549 }
550 
551 
552 bool MotorThread::stereoToCartesianNetwork(const Vector &stereo, Vector &xd)
553 {
554  if(stereo.size()!=4 || stereo[0]==0.0 || stereo[1]==0.0 || stereo[2]==0.0 || stereo[3]==0.0)
555  return false;
556 
557  Vector h(6);
558  enc_head->getEncoders(h.data());
559 
560  Vector in(7);
561  in[0]=stereo[0];
562  in[1]=stereo[1];
563  in[2]=stereo[2];
564  in[3]=stereo[3];
565  in[4]=h[3];
566  in[5]=h[4];
567  in[6]=h[5];
568 
569  xd=eye2root(net.predict(in),true);
570 
571  return xd.size()==3;
572 }
573 
574 
575 Vector MotorThread::randomDeployOffset()
576 {
577  Vector offset(3);
578  offset[0]=Rand::scalar(-0.01,0.0);
579  offset[1]=Rand::scalar(-0.02,0.02);
580  offset[2]=0.0;
581 
582  return offset;
583 }
584 
585 
586 bool MotorThread::loadKinematicOffsets()
587 {
588  ifstream kin_fin(rf.findFile(kinematics_file).c_str());
589  if (!kin_fin.is_open())
590  {
591  yError("Kinematics file not found!");
592  return false;
593  }
594 
595  stringstream strstr;
596  strstr<<kin_fin.rdbuf();
597 
598  bKinOffsets.fromString(strstr.str());
599 
600  if (!bKinOffsets.check("table_height"))
601  {
602  Bottle &bKinList=bKinOffsets.addList();
603  bKinList.addString("table_height");
604  bKinList.addFloat64(-0.1);
605  }
606 
607  table_height=bKinOffsets.find("table_height").asFloat64();
608  table.resize(4,0.0);
609  table[2]=1.0;
610  table[3]=-table_height;
611 
612  //adjust the table height accordingly to a specified tolerance
613  table_height+=table_height_tolerance;
614 
615  if (!bKinOffsets.check("left"))
616  {
617  Bottle &bKinList=bKinOffsets.addList();
618  bKinList.addString("left");
619 
620  bKinList.addList();
621  }
622 
623  if (!bKinOffsets.check("right"))
624  {
625  Bottle &bKinList=bKinOffsets.addList();
626  bKinList.addString("right");
627 
628  bKinList.addList();
629  }
630 
631  kin_fin.close();
632 
633  defaultKinematicOffset[LEFT].resize(3);
634  defaultKinematicOffset[RIGHT].resize(3);
635 
636  return true;
637 }
638 
639 
640 bool MotorThread::saveKinematicOffsets()
641 {
642  string fileName=rf.getHomeContextPath();
643  fileName+="/"+kinematics_file;
644  ofstream kin_fout(fileName.c_str());
645  if (!kin_fout.is_open())
646  {
647  yError("Unable to open file '%s'!",fileName.c_str());
648  return false;
649  }
650  else
651  {
652  kin_fout<<bKinOffsets.toString()<<endl;
653  kin_fout.close();
654  return true;
655  }
656 }
657 
658 
659 bool MotorThread::getGeneralOptions(Bottle &b)
660 {
661  if (Bottle *pB=b.find("home_fixation_point").asList())
662  {
663  homeFixCartType=true;
664  Value v=pB->get(0);
665  int offs=0;
666 
667  if (v.isString())
668  {
669  homeFixCartType=(v.asString()=="xyz");
670  offs++;
671  }
672 
673  homeFix.resize(pB->size()-offs);
674  for (size_t i=0; i<homeFix.length(); i++)
675  homeFix[i]=pB->get(offs+i).asFloat64();
676  }
677  else
678  return false;
679 
680  if (Bottle *pB=b.find("reach_above_displacement").asList())
681  {
682  reachAboveDisp.resize(pB->size());
683 
684  for (int i=0; i<pB->size(); i++)
685  reachAboveDisp[i]=pB->get(i).asFloat64();
686  }
687  else
688  return false;
689 
690  if (Bottle *pB=b.find("grasp_above_relief").asList())
691  {
692  graspAboveRelief.resize(pB->size());
693 
694  for (int i=0; i<pB->size(); i++)
695  graspAboveRelief[i]=pB->get(i).asFloat64();
696  }
697  else
698  return false;
699 
700  if (Bottle *pB=b.find("push_above_relief").asList())
701  {
702  pushAboveRelief.resize(pB->size());
703 
704  for (int i=0; i<pB->size(); i++)
705  pushAboveRelief[i]=pB->get(i).asFloat64();
706  }
707  else
708  return false;
709 
710  go_up_distance=b.check("go_up",Value(0.1)).asFloat64();
711  table_height_tolerance=b.find("table_height_tolerance").asFloat64();
712 
713  return true;
714 }
715 
716 
717 bool MotorThread::getArmOptions(Bottle &b, const int &arm)
718 {
719  if (Bottle *pB=b.find("home_position").asList())
720  {
721  homePos[arm].resize(pB->size());
722 
723  for (int i=0; i<pB->size(); i++)
724  homePos[arm][i]=pB->get(i).asFloat64();
725  }
726  else
727  return false;
728 
729  if (Bottle *pB=b.find("home_orientation").asList())
730  {
731  homeOrient[arm].resize(pB->size());
732 
733  for (int i=0; i<pB->size(); i++)
734  homeOrient[arm][i]=pB->get(i).asFloat64();
735  }
736  else
737  return false;
738 
739  if (Bottle *pB=b.find("reach_side_displacement").asList())
740  {
741  reachSideDisp[arm].resize(pB->size());
742 
743  for (int i=0; i<pB->size(); i++)
744  reachSideDisp[arm][i]=pB->get(i).asFloat64();
745  }
746  else
747  return false;
748 
749  if (Bottle *pB=b.find("reach_above_orientation").asList())
750  {
751  reachAboveOrient[arm].resize(pB->size());
752 
753  for (int i=0; i<pB->size(); i++)
754  reachAboveOrient[arm][i]=pB->get(i).asFloat64();
755  }
756  else
757  return false;
758 
759  if (Bottle *pB=b.find("reach_above_calib_table").asList())
760  {
761  reachAboveCata[arm].resize(pB->size());
762 
763  for (int i=0; i<pB->size(); i++)
764  reachAboveCata[arm][i]=pB->get(i).asFloat64();
765  }
766  else
767  return false;
768 
769  if (Bottle *pB=b.find("reach_side_orientation").asList())
770  {
771  reachSideOrient[arm].resize(pB->size());
772 
773  for (int i=0; i<pB->size(); i++)
774  reachSideOrient[arm][i]=pB->get(i).asFloat64();
775  }
776  else
777  return false;
778 
779  if (Bottle *pB=b.find("deploy_position").asList())
780  {
781  deployPos[arm].resize(pB->size());
782 
783  for (int i=0; i<pB->size(); i++)
784  deployPos[arm][i]=pB->get(i).asFloat64();
785  }
786  else
787  return false;
788 
789  if (Bottle *pB=b.find("deploy_orientation").asList())
790  {
791  deployOrient[arm].resize(pB->size());
792  for (int i=0; i<pB->size(); i++)
793  deployOrient[arm][i]=pB->get(i).asFloat64();
794  }
795  else
796  deployOrient[arm]=reachAboveOrient[arm];
797 
798  if (Bottle *pB=b.find("draw_near_position").asList())
799  {
800  drawNearPos[arm].resize(pB->size());
801 
802  for (int i=0; i<pB->size(); i++)
803  drawNearPos[arm][i]=pB->get(i).asFloat64();
804  }
805  else
806  return false;
807 
808  if (Bottle *pB=b.find("draw_near_orientation").asList())
809  {
810  drawNearOrient[arm].resize(pB->size());
811 
812  for (int i=0; i<pB->size(); i++)
813  drawNearOrient[arm][i]=pB->get(i).asFloat64();
814  }
815  else
816  return false;
817 
818  if (Bottle *pB=b.find("shift_position").asList())
819  {
820  shiftPos[arm].resize(pB->size());
821 
822  for (int i=0; i<pB->size(); i++)
823  shiftPos[arm][i]=pB->get(i).asFloat64();
824  }
825  else
826  {
827  shiftPos[arm].resize(3);
828  shiftPos[arm]=0.0;
829  }
830 
831  if (Bottle *pB=b.find("expect_position").asList())
832  {
833  expectPos[arm].resize(pB->size());
834 
835  for (int i=0; i<pB->size(); i++)
836  expectPos[arm][i]=pB->get(i).asFloat64();
837  }
838  else
839  return false;
840 
841  if (Bottle *pB=b.find("expect_orientation").asList())
842  {
843  expectOrient[arm].resize(pB->size());
844 
845  for (int i=0; i<pB->size(); i++)
846  expectOrient[arm][i]=pB->get(i).asFloat64();
847  }
848  else
849  return false;
850 
851  if (Bottle *pB=b.find("tool_take_position").asList())
852  {
853  takeToolPos[arm].resize(pB->size());
854 
855  for (int i=0; i<pB->size(); i++)
856  takeToolPos[arm][i]=pB->get(i).asFloat64();
857  }
858  else
859  return false;
860 
861  if (Bottle *pB=b.find("tool_take_orientation").asList())
862  {
863  takeToolOrient[arm].resize(pB->size());
864 
865  for (int i=0; i<pB->size(); i++)
866  takeToolOrient[arm][i]=pB->get(i).asFloat64();
867  }
868  else
869  return false;
870 
871  extForceThresh[arm]=b.check("external_forces_thresh",Value(0.0)).asFloat64();
872 
873  string modelFile("grasp_model_");
874  modelFile+=(arm==LEFT?"left":"right");
875  modelFile+=".ini";
876  graspFile[arm]=b.check("grasp_model_file",Value(modelFile)).asString();
877 
878  return true;
879 }
880 
881 
882 void MotorThread::close()
883 {
884  if (closed)
885  return;
886 
887  //set the system back to velocity mode
888  setImpedance(false);
889 
890  delete drv_head;
891  drv_head=NULL;
892 
893  delete drv_torso;
894  drv_torso=NULL;
895 
896  delete drv_arm[LEFT];
897  drv_arm[LEFT]=NULL;
898 
899  delete drv_arm[RIGHT];
900  drv_arm[RIGHT]=NULL;
901 
902  delete drv_ctrl_gaze;
903  drv_ctrl_gaze=NULL;
904 
905  delete action[LEFT];
906  action[LEFT]=NULL;
907 
908  delete action[RIGHT];
909  action[RIGHT]=NULL;
910 
911  disparityPort.interrupt();
912  disparityPort.close();
913 
914  wbdPort.interrupt();
915  wbdPort.close();
916 
917  wrenchPort[LEFT].interrupt();
918  wrenchPort[RIGHT].interrupt();
919  wrenchPort[LEFT].close();
920  wrenchPort[RIGHT].close();
921 
922  closed=true;
923 }
924 
925 
926 bool MotorThread::threadInit()
927 {
928  Bottle bMotor=rf.findGroup("motor");
929  if(bMotor.isNull())
930  {
931  yError("Motor part is missing!");
932  return false;
933  }
934 
935  string name=rf.find("name").asString();
936  string robot=bMotor.check("robot",Value("icub")).asString();
937  string partUsed=bMotor.check("part_used",Value("both_arms")).asString();
938  int actionprimitives_layer=bMotor.check("actionprimitives_layer",Value(2)).asInt32();
939  setPeriod((double)bMotor.check("thread_period",Value(100)).asInt32()/1000.0);
940 
941  actions_path=bMotor.check("actions",Value("actions")).asString();
942 
943  double eyesTrajTime=bMotor.check("eyes_traj_time",Value(1.0)).asFloat64();
944  double neckTrajTime=bMotor.check("neck_traj_time",Value(2.0)).asFloat64();
945 
946  double kp=bMotor.check("stereo_kp",Value(0.001)).asFloat64();
947  double ki=bMotor.check("stereo_ki",Value(0.001)).asFloat64();
948  double kd=bMotor.check("stereo_kd",Value(0.0)).asFloat64();
949 
950  stereo_track=bMotor.check("stereo_track",Value("on")).asString()=="on";
951  dominant_eye=(bMotor.check("dominant_eye",Value("left")).asString()=="left")?LEFT:RIGHT;
952 
953  Bottle *neckPitchRange=bMotor.find("neck_pitch_range").asList();
954  Bottle *neckRollRange=bMotor.find("neck_roll_range").asList();
955 
956  // open ports
957  disparityPort.open("/"+name+"/disparity:io");
958  wbdPort.open("/"+name+"/wbd:rpc");
959  wrenchPort[LEFT].open("/"+name+"/left/wrench:o");
960  wrenchPort[RIGHT].open("/"+name+"/right/wrench:o");
961 
962  // open controllers
963  Property optHead("(device remote_controlboard)");
964  Property optLeftArm("(device remote_controlboard)");
965  Property optRightArm("(device remote_controlboard)");
966  Property optTorso("(device remote_controlboard)");
967  Property optctrl_gaze("(device gazecontrollerclient)");
968 
969  optHead.put("remote","/"+robot+"/head");
970  optHead.put("local","/"+name+"/head");
971 
972  optLeftArm.put("remote","/"+robot+"/left_arm");
973  optLeftArm.put("local","/"+name+"/left_arm");
974 
975  optRightArm.put("remote","/"+robot+"/right_arm");
976  optRightArm.put("local","/"+name+"/right_arm");
977 
978  optTorso.put("remote","/"+robot+"/torso");
979  optTorso.put("local","/"+name+"/torso");
980 
981  optctrl_gaze.put("remote","/iKinGazeCtrl");
982  optctrl_gaze.put("local","/"+name+"/gaze");
983 
984  drv_head=new PolyDriver;
985  drv_torso=new PolyDriver;
986  drv_ctrl_gaze=new PolyDriver;
987  if (!drv_head->open(optHead) ||
988  !drv_torso->open(optTorso) ||
989  !drv_ctrl_gaze->open(optctrl_gaze))
990  {
991  close();
992  return false;
993  }
994 
995  // open views
996  drv_head->view(enc_head);
997  drv_torso->view(enc_torso);
998  drv_torso->view(pos_torso);
999  drv_torso->view(vel_torso);
1000  drv_torso->view(ctrl_mode_torso);
1001  drv_torso->view(int_mode_torso);
1002  drv_torso->view(ctrl_impedance_torso);
1003 
1004  if(partUsed=="both_arms" || partUsed=="left_arm")
1005  {
1006  drv_arm[LEFT]=new PolyDriver;
1007  if(!drv_arm[LEFT]->open(optLeftArm))
1008  {
1009  close();
1010  return false;
1011  }
1012 
1013  drv_arm[LEFT]->view(ctrl_mode_arm[LEFT]);
1014  drv_arm[LEFT]->view(int_mode_arm[LEFT]);
1015  drv_arm[LEFT]->view(ctrl_impedance_arm[LEFT]);
1016  drv_arm[LEFT]->view(pos_arm[LEFT]);
1017  drv_arm[LEFT]->view(enc_arm[LEFT]);
1018 
1019  Vector vels(16),accs(16);
1020  vels=20.0; accs=6000.0;
1021  pos_arm[LEFT]->setRefSpeeds(vels.data());
1022  pos_arm[LEFT]->setRefAccelerations(accs.data());
1023  }
1024 
1025  if(partUsed=="both_arms" || partUsed=="right_arm")
1026  {
1027  drv_arm[RIGHT]=new PolyDriver;
1028  if(!drv_arm[RIGHT]->open(optRightArm))
1029  {
1030  close();
1031  return false;
1032  }
1033 
1034  drv_arm[RIGHT]->view(ctrl_mode_arm[RIGHT]);
1035  drv_arm[RIGHT]->view(int_mode_arm[RIGHT]);
1036  drv_arm[RIGHT]->view(ctrl_impedance_arm[RIGHT]);
1037  drv_arm[RIGHT]->view(pos_arm[RIGHT]);
1038  drv_arm[RIGHT]->view(enc_arm[RIGHT]);
1039 
1040  Vector vels(16),accs(16);
1041  vels=20.0; accs=6000.0;
1042  pos_arm[RIGHT]->setRefSpeeds(vels.data());
1043  pos_arm[RIGHT]->setRefAccelerations(accs.data());
1044  }
1045 
1046  drv_ctrl_gaze->view(ctrl_gaze);
1047 
1048  Vector vels(3),accs(3);
1049  vels=5.0; accs=6000.0;
1050  pos_torso->setRefSpeeds(vels.data());
1051  pos_torso->setRefAccelerations(accs.data());
1052 
1053  // initialize the gaze controller
1054 
1055  //store the current context
1056  int initial_gaze_context;
1057  ctrl_gaze->storeContext(&initial_gaze_context);
1058 
1059  ctrl_gaze->setTrackingMode(false);
1060  ctrl_gaze->setEyesTrajTime(eyesTrajTime);
1061  ctrl_gaze->setNeckTrajTime(neckTrajTime);
1062 
1063  // set the values for the stereo PID controller
1064  Bottle stereoOpt;
1065  Bottle &bKp=stereoOpt.addList();
1066  bKp.addString("Kp");
1067  Bottle &bKpVal=bKp.addList();
1068  bKpVal.addFloat64(kp);
1069 
1070  Bottle &bKi=stereoOpt.addList();
1071  bKi.addString("Ki");
1072  Bottle &bKiVal=bKi.addList();
1073  bKiVal.addFloat64(ki);
1074 
1075  Bottle &bKd=stereoOpt.addList();
1076  bKd.addString("Kd");
1077  Bottle &bKdVal=bKd.addList();
1078  bKdVal.addFloat64(kd);
1079 
1080  Bottle &bTs=stereoOpt.addList();
1081  bTs.addString("Ts");
1082  bTs.addFloat64(0.05);
1083 
1084  Bottle &bDominantEye=stereoOpt.addList();
1085  bDominantEye.addString("dominantEye");
1086  dominant_eye==LEFT?bDominantEye.addString("left"):bDominantEye.addString("right");
1087 
1088  ctrl_gaze->setStereoOptions(stereoOpt);
1089 
1090  //bind neck pitch and roll;
1091  if(neckPitchRange!=NULL && neckPitchRange->size()==1)
1092  {
1093  double neckPitchBlock=neckPitchRange->get(0).asFloat64();
1094  ctrl_gaze->blockNeckPitch(neckPitchBlock);
1095  }
1096  else if(neckPitchRange!=NULL && neckPitchRange->size()>1)
1097  {
1098  double neckPitchMin=neckPitchRange->get(0).asFloat64();
1099  double neckPitchMax=neckPitchRange->get(1).asFloat64();
1100  ctrl_gaze->bindNeckPitch(neckPitchMin,neckPitchMax);
1101  }
1102  if(neckRollRange!=NULL && neckRollRange->size()==1)
1103  {
1104  double neckRollBlock=neckRollRange->get(0).asFloat64();
1105  ctrl_gaze->blockNeckRoll(neckRollBlock);
1106  }
1107  else if(neckRollRange!=NULL && neckRollRange->size()>1)
1108  {
1109  double neckRollMin=neckRollRange->get(0).asFloat64();
1110  double neckRollMax=neckRollRange->get(1).asFloat64();
1111  ctrl_gaze->bindNeckRoll(neckRollMin,neckRollMax);
1112  }
1113 
1114  if (bMotor.check("block_eyes"))
1115  {
1116  ctrl_gaze->blockEyes(bMotor.find("block_eyes").asFloat64());
1117  ctrl_gaze->waitMotionDone();
1118  }
1119 
1120  //store the current context and restore the initial one
1121  ctrl_gaze->storeContext(&gaze_context);
1122  ctrl_gaze->restoreContext(initial_gaze_context);
1123  ctrl_gaze->deleteContext(initial_gaze_context);
1124  gazeUnderControl=false;
1125 
1126  //-------------------------------
1127 
1128  // extract the exploration poses from a .ini
1129  string exploration_name=bMotor.find("exploration_poses").asString();
1130  if(!loadExplorationPoses(exploration_name))
1131  {
1132  yError("Error while loading exploration poses!");
1133  close();
1134  return false;
1135  }
1136 
1137  // init the NN
1138  Property netOptions;
1139  string net_name=bMotor.find("net").asString();
1140  netOptions.fromConfigFile(rf.findFile(net_name));
1141  if(net.configure(netOptions))
1142  {
1143  yInfo("Neural network configured successfully");
1144  neuralNetworkAvailable=true;
1145  }
1146  else
1147  {
1148  yError("Error while loading neural network!");
1149  neuralNetworkAvailable=false;
1150  }
1151 
1152  // get the general options
1153  if(!getGeneralOptions(bMotor))
1154  {
1155  yError("Error extracting general options!");
1156  close();
1157  return false;
1158  }
1159 
1160  Bottle bArm[2];
1161  bArm[LEFT]=rf.findGroup("left_arm");
1162  bArm[RIGHT]=rf.findGroup("right_arm");
1163 
1164  // parsing general arm config options
1165  Property option;
1166  for (int i=1; i<bMotor.size(); i++)
1167  {
1168  Bottle *pB=bMotor.get(i).asList();
1169  if (pB->size()==2)
1170  {
1171  if(pB->get(0).asString()=="hand_sequences_file")
1172  {
1173  string hand_seq_name=bMotor.find("hand_sequences_file").asString();
1174  option.put("hand_sequences_file",rf.findFile(hand_seq_name));
1175  }
1176  else
1177  option.put(pB->get(0).asString(),pB->get(1));
1178  }
1179  else
1180  {
1181  yError("Error: invalid option!");
1182  close();
1183  return false;
1184  }
1185  }
1186 
1187  //torso impedence values
1188  vector<double> torso_stiffness(0),torso_damping(0);
1189  Bottle *bImpedanceTorsoStiff=bMotor.find("impedence_torso_stiffness").asList();
1190  Bottle *bImpedanceTorsoDamp=bMotor.find("impedence_torso_damping").asList();
1191 
1192  for(int i=0; i<bImpedanceTorsoStiff->size(); i++)
1193  {
1194  torso_stiffness.push_back(bImpedanceTorsoStiff->get(i).asFloat64());
1195  torso_damping.push_back(bImpedanceTorsoDamp->get(i).asFloat64());
1196  }
1197 
1198  for(int i=0; i<bImpedanceTorsoStiff->size(); i++)
1199  ctrl_impedance_torso->setImpedance(i,torso_stiffness[i],torso_damping[i]);
1200 
1201  //arm impedence values
1202  vector<double> arm_stiffness(0),arm_damping(0);
1203  Bottle *bImpedanceArmStiff=bMotor.find("impedence_arm_stiffness").asList();
1204  Bottle *bImpedanceArmDamp=bMotor.find("impedence_arm_damping").asList();
1205 
1206  for(int i=0; i<bImpedanceArmStiff->size(); i++)
1207  {
1208  arm_stiffness.push_back(bImpedanceArmStiff->get(i).asFloat64());
1209  arm_damping.push_back(bImpedanceArmDamp->get(i).asFloat64());
1210  }
1211 
1212  option.put("local",name);
1213 
1214  default_exec_time=option.check("default_exec_time",Value("3.0")).asFloat64();
1215  reachingTimeout=std::max(2.0*default_exec_time,4.0);
1216 
1217  string arm_name[]={"left_arm","right_arm"};
1218  for (int arm=0; arm<2; arm++)
1219  {
1220  if (partUsed=="both_arms" || (partUsed=="left_arm" && arm==LEFT)
1221  || (partUsed=="right_arm" && arm==RIGHT))
1222  {
1223  // parsing arm-dependent config options
1224  if (bArm[arm].isNull())
1225  {
1226  yError("Missing %s parameter list!",arm_name[arm].c_str());
1227  close();
1228  return false;
1229  }
1230  else if(!getArmOptions(bArm[arm],arm))
1231  {
1232  yError("Error extracting %s options!",arm_name[arm].c_str());
1233  close();
1234  return false;
1235  }
1236 
1237  Property option_tmp(option);
1238  option_tmp.put("part",arm_name[arm]);
1239 
1240  string tmpGraspFile=rf.findFile(bArm[arm].find("grasp_model_file").asString());
1241  option_tmp.put("grasp_model_file",tmpGraspFile);
1242 
1243  if (actionprimitives_layer==1)
1244  {
1245  yInfo("***** Instantiating primitives layer 1 for %s",arm_name[arm].c_str());
1246  action[arm]=new ActionPrimitivesLayer1(option_tmp);
1247  }
1248  else
1249  {
1250  yInfo("***** Instantiating primitives layer 2 for %s",arm_name[arm].c_str());
1251  action[arm]=new ActionPrimitivesLayer2(option_tmp);
1252  }
1253  if (!action[arm]->isValid())
1254  {
1255  close();
1256  return false;
1257  }
1258 
1259  action[arm]->getGraspModel(graspModel[arm]);
1260  action[arm]->enableReachingTimeout(reachingTimeout);
1261 
1262  if (auto* p = dynamic_cast<ActionPrimitivesLayer2*>(action[arm]))
1263  p->setExtForceThres(extForceThresh[arm]);
1264 
1265  deque<string> q=action[arm]->getHandSeqList();
1266  yInfo("***** List of available %s hand sequence keys:",arm_name[arm].c_str());
1267  for (size_t i=0; i<q.size(); i++)
1268  {
1269  Bottle sequence;
1270  action[arm]->getHandSequence(q[i],sequence);
1271 
1272  yInfo("***** %s:",q[i].c_str());
1273  yInfo("%s",sequence.toString().c_str());
1274  }
1275 
1276  ICartesianControl *tmpCtrl;
1277  action[arm]->getCartesianIF(tmpCtrl);
1278 
1279  int context;
1280  tmpCtrl->storeContext(&context);
1281 
1282  double armTargetTol=bMotor.check("arm_target_tol",Value(0.01)).asFloat64();
1283  tmpCtrl->setInTargetTol(armTargetTol);
1284 
1285  storeContext(arm);
1286  tmpCtrl->restoreContext(context);
1287  tmpCtrl->deleteContext(context);
1288 
1289  // set elbow parameters
1290  if (Bottle *pB=bArm[arm].find("elbow_height").asList())
1291  {
1292  if (pB->size()>=2)
1293  {
1294  double height=pB->get(0).asFloat64();
1295  double weight=pB->get(1).asFloat64();
1296  changeElbowHeight(arm,height,weight);
1297  }
1298  }
1299 
1300  for(int i=0; i<bImpedanceArmStiff->size(); i++)
1301  ctrl_impedance_arm[arm]->setImpedance(i,arm_stiffness[i],arm_damping[i]);
1302 
1303  armInUse=arm;
1304  }
1305  }
1306 
1307  //set impedance on or off
1308  status_impedance_on=false;
1309  bool impedance_from_start=bMotor.check("impedance",Value("off")).asString()=="on";
1310  setImpedance(impedance_from_start);
1311  yInfo("Impedance set to %s",(status_impedance_on?"on":"off"));
1312 
1313  //init the kinematics offsets and table height
1314  kinematics_file=bMotor.find("kinematics_file").asString();
1315  if(!loadKinematicOffsets())
1316  {
1317  close();
1318  return false;
1319  }
1320 
1321  //set the initial stereo2cartesian mode
1322  int starting_modeS2C=bMotor.check("stereoTocartesian_mode",Value("homography")).asVocab32();
1323  setStereoToCartesianMode(starting_modeS2C);
1324 
1325  // initializer dragger
1326  dragger.init(rf.findGroup("dragger"),(int)(1000.0*this->getPeriod()));
1327 
1328  this->avoidTable(false);
1329 
1330  grasp_state=GRASP_STATE_IDLE;
1331 
1332  Rand::init();
1333 
1334  head_mode=HEAD_MODE_IDLE;
1335  arm_mode=ARM_MODE_IDLE;
1336 
1337  random_pos_y=bMotor.check("random_pos_y",Value(0.1)).asFloat64();
1338 
1339  closed=false;
1340  interrupted=false;
1341 
1342  this->setWaveing(bMotor.check("waveing",Value("off")).asString()=="on");
1343 
1344  return true;
1345 }
1346 
1347 
1348 void MotorThread::run()
1349 {
1350  //check if the system needs to be updated
1351  update();
1352 
1353  switch(head_mode)
1354  {
1355  case(HEAD_MODE_TRACK_HAND):
1356  {
1357  if (!gazeUnderControl)
1358  gazeUnderControl=true;
1359 
1360  Vector x,o;
1361  action[armInUse]->getPose(x,o);
1362  ctrl_gaze->lookAtFixationPoint(x);
1363  break;
1364  }
1365 
1366  case(HEAD_MODE_TRACK_TEMP):
1367  {
1368  Vector stereo=stereo_target.get();
1369  if(stereo.size()==4)
1370  {
1371  if(stereo_track)
1372  {
1373  Vector px[2];
1374  px[LEFT].resize(2);
1375  px[RIGHT].resize(2);
1376 
1377  px[LEFT][0]=stereo[0];
1378  px[LEFT][1]=stereo[1];
1379  px[RIGHT][0]=stereo[2];
1380  px[RIGHT][1]=stereo[3];
1381 
1382  ctrl_gaze->lookAtStereoPixels(px[LEFT],px[RIGHT]);
1383  }
1384  else
1385  {
1386  int eye_in_use;
1387  if(stereo[2*dominant_eye]==0.0 && stereo[2*dominant_eye+1]==0.0)
1388  {
1389  if(stereo[2*(1-dominant_eye)]==0.0 && stereo[2*(1-dominant_eye)+1]==0.0)
1390  break;
1391 
1392  eye_in_use=1-dominant_eye;
1393  }
1394  else
1395  eye_in_use=dominant_eye;
1396 
1397  Vector px(2);
1398  px[0]=stereo[2*eye_in_use];
1399  px[1]=stereo[2*eye_in_use+1];
1400  ctrl_gaze->lookAtMonoPixel(dominant_eye,px,0.4);
1401  }
1402  }
1403  break;
1404  }
1405 
1406  case(HEAD_MODE_TRACK_CART):
1407  {
1408  ctrl_gaze->lookAtFixationPoint(track_cartesian_target);
1409  break;
1410  }
1411 
1412  case(HEAD_MODE_TRACK_FIX):
1413  {
1414  if (!gazeUnderControl)
1415  gazeUnderControl=true;
1416  break;
1417  }
1418  }
1419 
1420  switch(arm_mode)
1421  {
1422  case(ARM_MODE_LEARN_ACTION):
1423  {
1424  Vector wrench, force(3);
1425 
1426  // get the wrench at the end-effector
1427  // and filter out forces under threshold
1428  if (auto* p = dynamic_cast<ActionPrimitivesLayer2*>(action[dragger.arm]))
1429  p->getExtWrench(wrench);
1430 
1431  force[0]=wrench[0];
1432  force[1]=wrench[1];
1433  force[2]=wrench[2];
1434 
1435  Vector x,o;
1436  dragger.ctrl->getPose(x,o);
1437 
1438  if(Time::now()-dragger.t0 > dragger.samplingRate)
1439  {
1440  //add the new position to the list of actions
1441  Bottle &tmp_action=dragger.actions.addList();
1442  Vector tmp_x=dragger.x0 - x;
1443 
1444  for(size_t i=0; i<tmp_x.size(); i++)
1445  tmp_action.addFloat64(tmp_x[i]);
1446 
1447  for(size_t i=0; i<o.size(); i++)
1448  tmp_action.addFloat64(o[i]);
1449 
1450  //here add timestamp to the files
1451  //tmp_action.addFloat64(dragger.t0);//temporary for the data collection
1452 
1453  dragger.t0=Time::now();
1454  }
1455 
1456  if(norm(force)>dragger.extForceThresh)
1457  x=x+0.1*force/norm(force);
1458 
1459  break;
1460  }
1461 
1462  case ARM_MODE_LEARN_KINOFF:
1463  {
1464  if(!dragger.using_impedance)
1465  {
1466  Vector wrench, force(3);
1467  double D=dragger.damping;
1468 
1469  // get the wrench at the end-effector
1470  // and filter out forces under threshold
1471  if (auto* p = dynamic_cast<ActionPrimitivesLayer2*>(action[dragger.arm]))
1472  p->getExtWrench(wrench);
1473 
1474  force[0]=wrench[0];
1475  force[1]=wrench[1];
1476  force[2]=wrench[2];
1477 
1478  if (norm(force)<dragger.extForceThresh)
1479  force=0.0;
1480  else
1481  D/=5.0;
1482 
1483  Vector b=force/dragger.inertia;
1484  Vector c=D*dragger.I->get();
1485  Vector a=force/dragger.inertia-D*dragger.I->get();
1486  Vector zeros4d(4);
1487  zeros4d=0.0;
1488 
1489  Vector v=dragger.I->integrate(a);
1490  dragger.ctrl->setTaskVelocities(v,zeros4d);
1491  }
1492 
1493  break;
1494  }
1495 
1496  case ARM_MODE_FINE_REACHING:
1497  {
1498  bool done;
1499  action[armInUse]->checkActionsDone(done,false);
1500  if(done)
1501  {
1502  Vector stereoHand=stereo_target.get();
1503  if(stereoHand.size()==4)
1504  {
1505  //move toward the target point
1506  }
1507 
1508  }
1509 
1510  break;
1511  }
1512 
1513  default:
1514  {
1515  //if the robot is asked to avoid the table with its arms
1516  if(avoid_table)
1517  {
1518  for(int arm=0; arm<2; arm++)
1519  {
1520  if(action[arm]!=NULL)
1521  {
1522  Vector x,o;
1523  action[arm]->getPose(x,o);
1524 
1525  Vector head_x,head_o;
1526  ctrl_gaze->getHeadPose(head_x,head_o);
1527 
1528  if(fabs(x[1]-head_x[1])<0.2)
1529  x[1]=head_x[1]+sign(x[1]-head_x[1])*0.2;
1530 
1531  ICartesianControl *tmp_ctrl;
1532  action[arm]->getCartesianIF(tmp_ctrl);
1533  x[2]=avoid_table_height[arm];
1534 
1535  tmp_ctrl->goToPosition(x);
1536  }
1537  }
1538  }
1539 
1540  break;
1541  }
1542  }
1543 }
1544 
1545 
1546 void MotorThread::threadRelease()
1547 {
1548  close();
1549 }
1550 
1551 
1552 void MotorThread::onStop()
1553 {
1554  if(action[LEFT]!=NULL)
1555  action[LEFT]->syncCheckInterrupt(true);
1556 
1557  if(action[RIGHT]!=NULL)
1558  action[RIGHT]->syncCheckInterrupt(true);
1559 }
1560 
1561 
1562 bool MotorThread::goUp(Bottle &options, const double h)
1563 {
1564  int arm=ARM_MOST_SUITED;
1565  if(checkOptions(options,"left") || checkOptions(options,"right"))
1566  arm=checkOptions(options,"left")?LEFT:RIGHT;
1567 
1568  arm=checkArm(arm);
1569 
1570  Vector x,o;
1571  action[arm]->getPose(x,o);
1572  x[2]+=(std::isnan(h)?go_up_distance:h);
1573 
1574  // no need to restore context (it's done in other methods)
1575 
1576  bool f;
1577  action[arm]->pushAction(x,o);
1578  action[arm]->checkActionsDone(f,true);
1579 
1580  return true;
1581 }
1582 
1583 
1584 bool MotorThread::reach(Bottle &options)
1585 {
1586  int arm=ARM_MOST_SUITED;
1587  if (checkOptions(options,"left") ||
1588  checkOptions(options,"right"))
1589  arm=checkOptions(options,"left")?LEFT:RIGHT;
1590 
1591  Bottle *bTarget=options.find("target").asList();
1592 
1593  Vector xd;
1594  if (!targetToCartesian(bTarget,xd))
1595  return false;
1596 
1597  arm=checkArm(arm,xd);
1598 
1599  if (!checkOptions(options,"no_head") &&
1600  !checkOptions(options,"no_gaze"))
1601  {
1602  setGazeIdle();
1603  options.addString("fixate");
1604  look(options);
1605  }
1606 
1607  bool side=checkOptions(options,"side");
1608  Vector tmpOrient,tmpDisp;
1609 
1610  if (side)
1611  {
1612  tmpOrient=reachSideOrient[arm];
1613  tmpDisp=reachSideDisp[arm];
1614  }
1615  else
1616  {
1617  tmpOrient=(checkOptions(options,"take")?
1618  reachAboveOrient[arm]:
1619  reachAboveCata[arm]);
1620  tmpDisp=reachAboveDisp;
1621 
1622  if (checkOptions(options,"touch"))
1623  xd[2]=std::min(xd[2],table_height-table_height_tolerance);
1624  }
1625 
1626  restoreContext(arm);
1627 
1628  wbdRecalibration();
1629  if (auto* p = dynamic_cast<ActionPrimitivesLayer2*>(action[arm]))
1630  p->enableContactDetection();
1631 
1632  ActionPrimitivesWayPoint wp;
1633  wp.x=xd+tmpDisp; wp.o=tmpOrient; wp.oEnabled=true;
1634  deque<ActionPrimitivesWayPoint> wpList;
1635  wpList.push_back(wp);
1636  wp.x=xd;
1637  wpList.push_back(wp);
1638  action[arm]->enableReachingTimeout(3.0*reachingTimeout);
1639 
1640  if (checkOptions(options,"pretake_hand"))
1641  action[arm]->pushAction(wpList,"pretake_hand");
1642  else
1643  action[arm]->pushAction(wpList);
1644 
1645  bool f;
1646  action[arm]->checkActionsDone(f,true);
1647  if (auto* p = dynamic_cast<ActionPrimitivesLayer2*>(action[arm]))
1648  {
1649  p->checkContact(f);
1650  p->disableContactDetection();
1651  }
1652 
1653  action[arm]->enableReachingTimeout(reachingTimeout);
1654 
1655  // go up straightaway
1656  if (f)
1657  {
1658  Vector x,o;
1659  action[arm]->getPose(x,o);
1660 
1661  if (!side)
1662  action[arm]->pushAction(x+graspAboveRelief,o);
1663 
1664  action[arm]->checkActionsDone(f,true);
1665  }
1666 
1667  setGraspState(side);
1668 
1669  if (!checkOptions(options,"no_head") &&
1670  !checkOptions(options,"no_gaze"))
1671  setGazeIdle();
1672 
1673  return true;
1674 }
1675 
1676 
1677 bool MotorThread::powerGrasp(Bottle &options)
1678 {
1679  int arm=ARM_MOST_SUITED;
1680  if(checkOptions(options,"left") || checkOptions(options,"right"))
1681  arm=checkOptions(options,"left")?LEFT:RIGHT;
1682 
1683  Bottle *bTarget=options.find("target").asList();
1684 
1685  Vector xd;
1686  if (!targetToCartesian(bTarget,xd))
1687  return false;
1688  if (xd.length()<7)
1689  return false;
1690 
1691  arm=checkArm(arm,xd,false);
1692  if (!checkOptions(options,"no_head") && !checkOptions(options,"no_gaze"))
1693  {
1694  setGazeIdle();
1695  options.addString("fixate");
1696  look(options);
1697  }
1698 
1699  Vector approach_data(4,0.0);
1700  if (options.check("approach"))
1701  {
1702  if (Bottle *bApproach=options.find("approach").asList())
1703  {
1704  size_t sz=std::min(approach_data.size(),(size_t)bApproach->size());
1705  for (size_t i=0; i<sz; i++)
1706  approach_data[i]=bApproach->get(i).asFloat64();
1707  }
1708  }
1709 
1710  Vector x=xd.subVector(0,2);
1711  Vector o=xd.subVector(3,6);
1712 
1713  Matrix R=axis2dcm(o);
1714  Vector y=R.getCol(1);
1715  y[3]=CTRL_DEG2RAD*approach_data[3];
1716 
1717  Vector dx=approach_data[0]*R.getCol(0).subVector(0,2);
1718  Vector dy=approach_data[1]*R.getCol(1).subVector(0,2);
1719  Vector dz=approach_data[2]*R.getCol(2).subVector(0,2);
1720 
1721  Vector approach_x=x+dx+dy+dz;
1722  Vector approach_o=dcm2axis(axis2dcm(y)*R);
1723 
1724  restoreContext(arm);
1725 
1726  wbdRecalibration();
1727  if (auto* p = dynamic_cast<ActionPrimitivesLayer2*>(action[arm]))
1728  p->enableContactDetection();
1729  action[arm]->enableReachingTimeout(0.6*reachingTimeout);
1730  action[arm]->pushAction(approach_x,approach_o,"pregrasp_hand");
1731 
1732  bool f;
1733  action[arm]->pushAction(x,o);
1734  action[arm]->checkActionsDone(f,true);
1735  action[arm]->enableReachingTimeout(reachingTimeout);
1736  if (auto* p = dynamic_cast<ActionPrimitivesLayer2*>(action[arm]))
1737  p->disableContactDetection();
1738 
1739  return grasp(options);
1740 }
1741 
1742 
1743 bool MotorThread::push(Bottle &options)
1744 {
1745  int arm=ARM_MOST_SUITED;
1746  if (checkOptions(options,"left") || checkOptions(options,"right"))
1747  arm=checkOptions(options,"left")?LEFT:RIGHT;
1748 
1749  Vector xd;
1750  if (!targetToCartesian(options.find("target").asList(),xd))
1751  return false;
1752  arm=checkArm(arm,xd);
1753 
1754  if (!checkOptions(options,"no_head") && !checkOptions(options,"no_gaze"))
1755  {
1756  setGazeIdle();
1757  options.addString("fixate");
1758  look(options);
1759  }
1760 
1761  xd[2]=table_height;
1762  xd+=pushAboveRelief;
1763  double push_direction=checkOptions(options,"away")?-1.0:1.0;
1764  Vector tmpDisp=push_direction*reachSideDisp[arm];
1765  Vector tmpOrient=reachSideOrient[arm];
1766 
1767  restoreContext(arm);
1768 
1769  bool f;
1770  action[arm]->pushAction(xd+tmpDisp,tmpOrient);
1771  action[arm]->checkActionsDone(f,true);
1772 
1773  if (!checkOptions(options,"no_head") && !checkOptions(options,"no_gaze"))
1774  {
1775  setGazeIdle();
1776  ctrl_gaze->restoreContext(gaze_context);
1777  keepFixation(options);
1778  lookAtHand(options);
1779  }
1780 
1781  wbdRecalibration();
1782  if (auto* p = dynamic_cast<ActionPrimitivesLayer2*>(action[arm]))
1783  p->enableContactDetection();
1784  action[arm]->pushAction(xd-3*push_direction*reachSideDisp[arm],reachSideOrient[arm]);
1785  action[arm]->checkActionsDone(f,true);
1786  if (auto* p = dynamic_cast<ActionPrimitivesLayer2*>(action[arm]))
1787  p->disableContactDetection();
1788 
1789  if (!checkOptions(options,"no_head") && !checkOptions(options,"no_gaze"))
1790  setGazeIdle();
1791 
1792  return true;
1793 }
1794 
1795 
1796 bool MotorThread::point(Bottle &options)
1797 {
1798  int arm=ARM_MOST_SUITED;
1799  if(checkOptions(options,"left") || checkOptions(options,"right"))
1800  arm=checkOptions(options,"left")?LEFT:RIGHT;
1801 
1802  Bottle *bTarget=options.find("target").asList();
1803 
1804  Vector xd;
1805  if(!targetToCartesian(bTarget,xd))
1806  return false;
1807 
1808  if(!checkOptions(options,"no_head") && !checkOptions(options,"no_gaze"))
1809  {
1810  setGazeIdle();
1811  look(options);
1812  }
1813 
1814  arm=checkArm(arm,xd);
1815  xd[0]=std::min(xd[0]+0.07,-0.15);
1816  xd[1]+=(arm==LEFT)?-0.05:0.05;
1817  xd[2]+=0.03;
1818 
1819  Matrix R=zeros(3,3);
1820  R(0,0)=-1.0;
1821  R(1,1)=(arm==LEFT)?-1.0:1.0;
1822  R(2,2)=(arm==LEFT)?1.0:-1.0;
1823 
1824  restoreContext(arm);
1825 
1826  Vector od=dcm2axis(R);
1827  action[arm]->pushAction(xd,od,"pointing_hand");
1828 
1829  bool f;
1830  action[arm]->checkActionsDone(f,true);
1831  if(!checkOptions(options,"no_head") && !checkOptions(options,"no_gaze"))
1832  {
1833  setGazeIdle();
1834  ctrl_gaze->setTrackingMode(false);
1835  }
1836 
1837  return true;
1838 }
1839 
1840 
1841 bool MotorThread::point_far(Bottle &options)
1842 {
1843  Vector xd;
1844  Bottle *bTarget=options.find("target").asList();
1845  if (!targetToCartesian(bTarget,xd))
1846  return false;
1847 
1848  int arm=ARM_MOST_SUITED;
1849  if (checkOptions(options,"left"))
1850  arm=LEFT;
1851  else if (checkOptions(options,"right"))
1852  arm=RIGHT;
1853  else
1854  arm=checkArm(arm,xd);
1855 
1856  restoreContext(arm);
1857  ICartesianControl* iarm;
1858  action[arm]->getCartesianIF(iarm);
1859 
1860  Property requirements;
1861  Bottle point; point.addList().read(xd);
1862  requirements.put("point",point.get(0));
1863 
1864  Bottle pointing_sequence;
1865  if (action[arm]->getHandSequence("pointing_hand",pointing_sequence))
1866  {
1867  int numWayPoints=pointing_sequence.find("numWayPoints").asInt32();
1868  ostringstream wp; wp<<"wp_"<<numWayPoints-1;
1869  Bottle *poss=pointing_sequence.find(wp.str()).asList()->find("poss").asList();
1870  Vector joints(poss->size());
1871  for (size_t i=0; i<joints.length(); i++)
1872  joints[i]=poss->get(i).asFloat64();
1873 
1874  Bottle finger_joints; finger_joints.addList().read(joints);
1875  requirements.put("finger-joints",finger_joints.get(0));
1876  }
1877 
1878  Vector q,x;
1879  if (PointingFar::compute(iarm,requirements,q,x))
1880  {
1881  if (!checkOptions(options,"no_head") &&
1882  !checkOptions(options,"no_gaze"))
1883  {
1884  setGazeIdle();
1885  options.addString("fixate");
1886  look(options);
1887  }
1888 
1889  action[arm]->pushAction("pointing_hand");
1890  PointingFar::point(iarm,q,x);
1891 
1892  if (!checkOptions(options,"no_head") &&
1893  !checkOptions(options,"no_gaze"))
1894  {
1895  setGazeIdle();
1896  ctrl_gaze->setTrackingMode(false);
1897  }
1898  }
1899 
1900  return true;
1901 }
1902 
1903 
1904 bool MotorThread::look(Bottle &options)
1905 {
1906  setGazeIdle();
1907  ctrl_gaze->restoreContext(gaze_context);
1908 
1909  if (checkOptions(options,"hand"))
1910  {
1911  int arm=ARM_IN_USE;
1912  if(checkOptions(options,"left") || checkOptions(options,"right"))
1913  arm=checkOptions(options,"left")?LEFT:RIGHT;
1914 
1915  arm=checkArm(arm);
1916  lookAtHand(options);
1917  }
1918  else
1919  {
1920  Vector xd;
1921  Bottle *bTarget=options.find("target").asList();
1922  if (!targetToCartesian(bTarget,xd))
1923  return false;
1924 
1925  if (options.check("block_eyes"))
1926  {
1927  ctrl_gaze->blockEyes(options.find("block_eyes").asFloat64());
1928  ctrl_gaze->waitMotionDone();
1929  }
1930 
1931  if (checkOptions(options,"fixate"))
1932  keepFixation(options);
1933 
1934  ctrl_gaze->lookAtFixationPointSync(xd);
1935  if (checkOptions(options,"wait"))
1936  ctrl_gaze->waitMotionDone();
1937  }
1938 
1939  return true;
1940 }
1941 
1942 
1943 bool MotorThread::takeTool(Bottle &options)
1944 {
1945  int arm=ARM_IN_USE;
1946  if (checkOptions(options,"left") || checkOptions(options,"right"))
1947  arm=checkOptions(options,"left")?LEFT:RIGHT;
1948 
1949  arm=checkArm(arm);
1950  if (!checkOptions(options,"no_head") && !checkOptions(options,"no_gaze"))
1951  {
1952  setGazeIdle();
1953  lookAtHand(options);
1954  }
1955 
1956  restoreContext(arm);
1957  action[arm]->pushAction(takeToolPos[arm],takeToolOrient[arm],"open_hand");
1958 
1959  bool f;
1960  action[arm]->checkActionsDone(f,true);
1961 
1962  double force_thresh;
1963  if (auto* p = dynamic_cast<ActionPrimitivesLayer2*>(action[arm]))
1964  p->getExtForceThres(force_thresh);
1965 
1966  bool contact_detected=false;
1967  Vector wrench(6);
1968  double t=Time::now();
1969 
1970  while (!contact_detected && (Time::now()-t<5.0))
1971  {
1972  if (auto* p = dynamic_cast<ActionPrimitivesLayer2*>(action[arm]))
1973  p->getExtWrench(wrench);
1974 
1975  if (norm(wrench)>force_thresh)
1976  contact_detected=true;
1977 
1978  Time::delay(0.1);
1979  }
1980 
1981  if (!checkOptions(options,"no_head") && !checkOptions(options,"no_gaze"))
1982  setGazeIdle();
1983 
1984  return true;
1985 }
1986 
1987 
1988 bool MotorThread::expect(Bottle &options)
1989 {
1990  int arm=ARM_IN_USE;
1991  if(checkOptions(options,"left") || checkOptions(options,"right"))
1992  arm=checkOptions(options,"left")?LEFT:RIGHT;
1993 
1994  arm=checkArm(arm);
1995 
1996  if(!checkOptions(options,"no_head") && !checkOptions(options,"no_gaze"))
1997  {
1998  setGazeIdle();
1999  lookAtHand(options);
2000  }
2001 
2002  restoreContext(arm);
2003  action[arm]->pushAction(expectPos[arm],expectOrient[arm],"open_hand");
2004 
2005  bool f;
2006  action[arm]->checkActionsDone(f,true);
2007 
2008  double force_thresh;
2009  if (auto* p = dynamic_cast<ActionPrimitivesLayer2*>(action[arm]))
2010  p->getExtForceThres(force_thresh);
2011 
2012  bool contact_detected=false;
2013  Vector wrench(6);
2014  double t=Time::now();
2015  while(!contact_detected && Time::now()-t<5.0)
2016  {
2017  if (auto* p = dynamic_cast<ActionPrimitivesLayer2*>(action[arm]))
2018  p->getExtWrench(wrench);
2019 
2020  if(norm(wrench)>force_thresh)
2021  contact_detected=true;
2022 
2023  Time::delay(0.1);
2024  }
2025 
2026  if(!checkOptions(options,"no_head") && !checkOptions(options,"no_gaze"))
2027  setGazeIdle();
2028 
2029  return true;
2030 }
2031 
2032 
2033 bool MotorThread::give(Bottle &options)
2034 {
2035  int arm=ARM_IN_USE;
2036  if(checkOptions(options,"left") || checkOptions(options,"right"))
2037  arm=checkOptions(options,"left")?LEFT:RIGHT;
2038 
2039  arm=checkArm(arm);
2040 
2041  if(!checkOptions(options,"no_head") && !checkOptions(options,"no_gaze"))
2042  {
2043  setGazeIdle();
2044  lookAtHand(options);
2045  }
2046 
2047  restoreContext(arm);
2048  action[arm]->pushAction(expectPos[arm],expectOrient[arm]);
2049 
2050  bool f;
2051  action[arm]->checkActionsDone(f,true);
2052  action[arm]->pushAction("open_hand");
2053 
2054  wbdRecalibration();
2055  if (auto* p = dynamic_cast<ActionPrimitivesLayer2*>(action[arm]))
2056  p->enableContactDetection();
2057 
2058  bool contact_detected=false;
2059  double t=Time::now();
2060  while (!contact_detected && (Time::now()-t<5.0))
2061  {
2062  if (auto* p = dynamic_cast<ActionPrimitivesLayer2*>(action[arm]))
2063  p->checkContact(contact_detected);
2064  Time::delay(0.1);
2065  }
2066 
2067  if (auto* p = dynamic_cast<ActionPrimitivesLayer2*>(action[arm]))
2068  p->disableContactDetection();
2069 
2070  if(!checkOptions(options,"no_head") && !checkOptions(options,"no_gaze"))
2071  setGazeIdle();
2072 
2073  return true;
2074 }
2075 
2076 
2077 bool MotorThread::clearIt(Bottle &options)
2078 {
2079  setGazeIdle();
2080  ctrl_gaze->setSaccadesMode(false);
2081  return true;
2082 }
2083 
2084 
2085 bool MotorThread::hand(const Bottle &options, const string &type,
2086  bool *holding)
2087 {
2088  int arm=ARM_IN_USE;
2089  if (checkOptions(options,"left") || checkOptions(options,"right"))
2090  arm=checkOptions(options,"left")?LEFT:RIGHT;
2091 
2092  arm=checkArm(arm);
2093 
2094  string action_type;
2095  if (type.empty())
2096  {
2097  if (options.size()<2)
2098  {
2099  yError("Too few arguments for HAND command!");
2100  return false;
2101  }
2102  action_type=options.get(1).asString();
2103  }
2104  else
2105  action_type=type;
2106 
2107  if (action[arm]->pushAction(action_type))
2108  {
2109  bool f;
2110  action[arm]->checkActionsDone(f,true);
2111  if (holding!=NULL)
2112  *holding=isHolding(options);
2113  return true;
2114  }
2115  else
2116  {
2117  yError("Unknown hand sequence \"%s\"",action_type.c_str());
2118  return false;
2119  }
2120 }
2121 
2122 
2123 bool MotorThread::grasp(const Bottle &options)
2124 {
2125  bool holding;
2126  if (hand(options,"close_hand",&holding))
2127  return holding;
2128  else
2129  return false;
2130 }
2131 
2132 
2133 bool MotorThread::release(const Bottle &options)
2134 {
2135  hand(options,"release_hand");
2136  return true;
2137 }
2138 
2139 
2140 bool MotorThread::changeElbowHeight(const int arm, const double height,
2141  const double weight)
2142 {
2143  bool ret=false;
2144  if (action[arm]!=NULL)
2145  {
2146  ICartesianControl *ctrl;
2147  action[arm]->getCartesianIF(ctrl);
2148 
2149  ctrl->stopControl();
2150 
2151  int context;
2152  ctrl->storeContext(&context);
2153 
2154  restoreContext(arm);
2155 
2156  Bottle tweakOptions;
2157  Bottle &optTask2=tweakOptions.addList();
2158  optTask2.addString("task_2");
2159  Bottle &plTask2=optTask2.addList();
2160  plTask2.addInt32(6);
2161  Bottle &posPart=plTask2.addList();
2162  posPart.addFloat64(0.0);
2163  posPart.addFloat64(0.0);
2164  posPart.addFloat64(height);
2165  Bottle &weightsPart=plTask2.addList();
2166  weightsPart.addFloat64(0.0);
2167  weightsPart.addFloat64(0.0);
2168  weightsPart.addFloat64(weight);
2169  ret=ctrl->tweakSet(tweakOptions);
2170 
2171  deleteContext(arm);
2172  storeContext(arm);
2173 
2174  ctrl->restoreContext(context);
2175  ctrl->deleteContext(context);
2176  }
2177 
2178  return ret;
2179 }
2180 
2181 
2182 bool MotorThread::changeExecTime(const int arm, const double execTime)
2183 {
2184  if (execTime>0.0)
2185  {
2186  default_exec_time=execTime;
2187  reachingTimeout=std::max(2.0*default_exec_time,4.0);
2188  if ((arm==LEFT) || (arm==RIGHT))
2189  {
2190  bool ret=false;
2191  if (action[arm]!=NULL)
2192  {
2193  ICartesianControl *ctrl;
2194  action[arm]->getCartesianIF(ctrl);
2195 
2196  ctrl->stopControl();
2197 
2198  int context;
2199  ctrl->storeContext(&context);
2200 
2201  restoreContext(arm);
2202 
2203  ret=action[LEFT]->setDefaultExecTime(execTime);
2204 
2205  deleteContext(arm);
2206  storeContext(arm);
2207 
2208  ctrl->restoreContext(context);
2209  ctrl->deleteContext(context);
2210  }
2211 
2212  return ret;
2213  }
2214  }
2215 
2216  return false;
2217 }
2218 
2219 
2220 void MotorThread::goWithTorsoUpright(ActionPrimitives *action,
2221  const Vector &xin, const Vector &oin)
2222 {
2223  ICartesianControl *ctrl;
2224  action->getCartesianIF(ctrl);
2225 
2226  int tmp_ctxt;
2227  ctrl->stopControl();
2228  ctrl->storeContext(&tmp_ctxt);
2229 
2230  Vector dof;
2231  ctrl->getDOF(dof); dof=1.0;
2232  ctrl->setDOF(dof,dof);
2233 
2234  ctrl->setLimits(0,0.0,0.0);
2235  ctrl->setLimits(1,0.0,0.0);
2236  ctrl->setLimits(2,0.0,0.0);
2237 
2238  ctrl->setTrajTime(default_exec_time);
2239  ctrl->setInTargetTol(0.04);
2240 
2241  ctrl->goToPoseSync(xin,oin);
2242  ctrl->waitMotionDone(0.1,reachingTimeout);
2243 
2244  ctrl->stopControl();
2245  ctrl->restoreContext(tmp_ctxt);
2246 }
2247 
2248 
2249 bool MotorThread::goHome(Bottle &options)
2250 {
2251  bool head_home=(checkOptions(options,"head") || checkOptions(options,"gaze"));
2252  bool arms_home=(checkOptions(options,"arms") || checkOptions(options,"arm"));
2253  bool hand_home=(checkOptions(options,"fingers") || checkOptions(options,"hands") ||
2254  checkOptions(options,"hand"));
2255 
2256  //if none is specified then assume all are going home
2257  if (!head_home && !arms_home && !hand_home)
2258  head_home=arms_home=hand_home=true;
2259 
2260  //workaround
2261  head_home = head_home && !checkOptions(options,"no_head") && !checkOptions(options,"no_gaze");
2262 
2263  bool left_arm=checkOptions(options,"left") || checkOptions(options,"both");
2264  bool right_arm=checkOptions(options,"right") || checkOptions(options,"both");
2265 
2266  //if none is specified then assume both arms (or hands) are going home
2267  if (!left_arm && !right_arm)
2268  left_arm=right_arm=true;
2269 
2270  if (head_home)
2271  {
2272  setGazeIdle();
2273  ctrl_gaze->restoreContext(gaze_context);
2274  ctrl_gaze->setTrackingMode(true);
2275 
2276  if (homeFixCartType)
2277  ctrl_gaze->lookAtFixationPointSync(homeFix);
2278  else
2279  ctrl_gaze->lookAtAbsAnglesSync(homeFix);
2280  }
2281 
2282  if (arms_home)
2283  {
2284  // first off, LEFT
2285  bool exec_arm[2];
2286  exec_arm[0]=left_arm;
2287  exec_arm[1]=right_arm;
2288  int which_arm[2]={ LEFT, RIGHT };
2289 
2290  // in case we're asked to deal with both
2291  // first off, take home the arm in use
2292  if (exec_arm[0] && exec_arm[1])
2293  {
2294  if (armInUse==RIGHT)
2295  {
2296  which_arm[0]=RIGHT;
2297  which_arm[1]=LEFT;
2298  }
2299  }
2300 
2301  for (size_t i=0; i<2; i++)
2302  {
2303  if (exec_arm[i] && (action[which_arm[i]]!=NULL))
2304  {
2305  if (hand_home)
2306  action[which_arm[i]]->pushAction("open_hand");
2307 
2308  goWithTorsoUpright(action[which_arm[i]],homePos[which_arm[i]],homeOrient[which_arm[i]]);
2309 
2310  bool f;
2311  action[which_arm[i]]->checkActionsDone(f,true);
2312  }
2313  }
2314 
2315  if (waveing && right_arm && (action[RIGHT]!=NULL))
2316  action[RIGHT]->enableArmWaving(homePos[RIGHT]);
2317 
2318  if (waveing && left_arm && (action[LEFT]!=NULL))
2319  action[LEFT]->enableArmWaving(homePos[LEFT]);
2320  }
2321  else if (hand_home)
2322  {
2323  if (left_arm)
2324  action[LEFT]->pushAction("open_hand");
2325 
2326  if (right_arm)
2327  action[RIGHT]->pushAction("open_hand");
2328  }
2329 
2330  if (head_home)
2331  {
2332  ctrl_gaze->waitMotionDone(0.1,3.0);
2333  ctrl_gaze->stopControl(); // because we're on tracking
2334  ctrl_gaze->setTrackingMode(false);
2335  }
2336 
2337  return true;
2338 }
2339 
2340 
2341 bool MotorThread::deploy(Bottle &options)
2342 {
2343  int arm=ARM_IN_USE;
2344  if (checkOptions(options,"left") || checkOptions(options,"right"))
2345  arm=checkOptions(options,"left")?LEFT:RIGHT;
2346 
2347  arm=checkArm(arm);
2348 
2349  Vector x,o;
2350  action[arm]->getPose(x,o);
2351 
2352  Bottle *bTarget=options.find("target").asList();
2353 
2354  Vector deployZone;
2355  if (!targetToCartesian(bTarget,deployZone))
2356  {
2357  deployZone=deployPos[arm];
2358  if (checkOptions(options,"away"))
2359  {
2360  deployZone[0]=-0.15;
2361  deployZone[1]=(arm==LEFT)?deployZone[1]=-0.35:deployZone[1]=0.35;
2362  }
2363 
2364  deployZone=deployZone+randomDeployOffset();
2365  deployZone[2]=table_height;
2366  }
2367  else
2368  {
2369  arm=checkArm(arm,deployZone);
2370  if (checkOptions(options,"gently") && !checkOptions(options,"over"))
2371  deployZone[2]=table_height;
2372  }
2373 
2374  if (!checkOptions(options,"no_head") && !checkOptions(options,"no_gaze"))
2375  {
2376  setGazeIdle();
2377  ctrl_gaze->restoreContext(gaze_context);
2378  keepFixation(options);
2379  ctrl_gaze->lookAtFixationPoint(deployZone);
2380  }
2381 
2382  Vector tmpOrient=(grasp_state==GRASP_STATE_SIDE?reachSideOrient[arm]:deployOrient[arm]);
2383 
2384  Vector deployOffs(deployZone.length(),0.0);
2385  deployOffs[2]=reachAboveDisp[2];
2386 
2387  restoreContext(arm);
2388 
2389  bool f;
2390  action[arm]->pushAction(deployZone+deployOffs,tmpOrient);
2391  action[arm]->checkActionsDone(f,true);
2392 
2393  wbdRecalibration();
2394  if (auto* p = dynamic_cast<ActionPrimitivesLayer2*>(action[arm]))
2395  p->enableContactDetection();
2396 
2397  ActionPrimitivesWayPoint wp;
2398  deployOffs*=0.5;
2399  wp.x=deployZone+deployOffs; wp.o=tmpOrient; wp.oEnabled=true;
2400  deque<ActionPrimitivesWayPoint> wpList;
2401  wpList.push_back(wp);
2402  wp.x=deployZone;
2403  wpList.push_back(wp);
2404  action[arm]->enableReachingTimeout(3.0*reachingTimeout);
2405  action[arm]->pushAction(wpList);
2406 
2407  action[arm]->checkActionsDone(f,true);
2408  action[arm]->getPose(x,o);
2409  if (auto* p = dynamic_cast<ActionPrimitivesLayer2*>(action[arm]))
2410  p->disableContactDetection();
2411 
2412  if (!checkOptions(options,"no_head") && !checkOptions(options,"no_gaze"))
2413  ctrl_gaze->lookAtFixationPoint(x);
2414 
2415  release(options);
2416  return true;
2417 }
2418 
2419 
2420 bool MotorThread::drawNear(Bottle &options)
2421 {
2422  int arm=ARM_IN_USE;
2423  if (checkOptions(options,"left") ||
2424  checkOptions(options,"right"))
2425  arm=checkOptions(options,"left")?LEFT:RIGHT;
2426 
2427  arm=checkArm(arm);
2428  goWithTorsoUpright(action[arm],drawNearPos[arm],drawNearOrient[arm]);
2429 
2430  Bottle look_options;
2431  Bottle &target=look_options.addList();
2432  target.addString("target");
2433  Bottle &payLoad=target.addList().addList();
2434  payLoad.addString("cartesian");
2435  payLoad.addList().read(drawNearPos[arm]);
2436  look_options.addString("wait");
2437  look(look_options);
2438 
2439  return true;
2440 }
2441 
2442 
2443 bool MotorThread::shiftAndGrasp(Bottle &options)
2444 {
2445  int arm=ARM_IN_USE;
2446  if (checkOptions(options,"left") ||
2447  checkOptions(options,"right"))
2448  arm=checkOptions(options,"left")?LEFT:RIGHT;
2449 
2450  arm=checkArm(arm);
2451 
2452  Vector x,o;
2453  action[arm]->getPose(x,o);
2454  x=x+shiftPos[arm];
2455 
2456  // no need to restore context (it's done in reach())
2457  action[arm]->pushAction(x,reachAboveOrient[arm],"close_hand");
2458 
2459  bool f;
2460  action[arm]->checkActionsDone(f,true);
2461 
2462  return true;
2463 }
2464 
2465 
2466 bool MotorThread::getHandImagePosition(Bottle &hand_image_pos)
2467 {
2468  int arm=ARM_IN_USE;
2469  arm=checkArm(arm);
2470 
2471  Vector x_hand,o_hand,px[2],x_head,o_head;
2472  action[arm]->getPose(x_hand,o_hand);
2473  ctrl_gaze->get2DPixel(LEFT,x_hand,px[LEFT]);
2474  ctrl_gaze->get2DPixel(RIGHT,x_hand,px[RIGHT]);
2475 
2476  ctrl_gaze->getHeadPose(x_head,o_head);
2477 
2478  hand_image_pos.clear();
2479  hand_image_pos.addFloat64(px[LEFT][0]);
2480  hand_image_pos.addFloat64(px[LEFT][1]);
2481  hand_image_pos.addFloat64(px[RIGHT][0]);
2482  hand_image_pos.addFloat64(px[RIGHT][1]);
2483  hand_image_pos.addFloat64(norm(x_head-x_hand));
2484 
2485  return true;
2486 }
2487 
2488 
2489 bool MotorThread::isHolding(const Bottle &options)
2490 {
2491  int arm=ARM_IN_USE;
2492  if (checkOptions(options,"left") ||
2493  checkOptions(options,"right"))
2494  arm=checkOptions(options,"left")?LEFT:RIGHT;
2495 
2496  arm=checkArm(arm);
2497 
2498  bool in_position;
2499  action[arm]->areFingersInPosition(in_position);
2500 
2501  return !in_position;
2502 }
2503 
2504 
2505 bool MotorThread::calibTable(Bottle &options)
2506 {
2507  int arm=ARM_IN_USE;
2508  if(checkOptions(options,"left") || checkOptions(options,"right"))
2509  arm=checkOptions(options,"left")?LEFT:RIGHT;
2510 
2511  arm=checkArm(arm);
2512 
2513  Vector deployZone=deployPos[arm];
2514  deployZone=deployZone+randomDeployOffset();
2515 
2516  Vector deployPrepare,deployEnd;
2517  deployPrepare=deployEnd=deployZone;
2518 
2519  deployPrepare[2]=0.0;
2520  deployEnd[2]=-0.25;
2521 
2522  bool f=false;
2523 
2524  if(!checkOptions(options,"no_head") && !checkOptions(options,"no_gaze"))
2525  {
2526  setGazeIdle();
2527  ctrl_gaze->restoreContext(gaze_context);
2528  ctrl_gaze->lookAtFixationPoint(deployEnd);
2529  }
2530 
2531  if(isHolding(options))
2532  action[arm]->pushAction("open_hand");
2533 
2534  restoreContext(arm);
2535 
2536  wbdRecalibration();
2537  if (auto* p = dynamic_cast<ActionPrimitivesLayer2*>(action[arm]))
2538  p->enableContactDetection();
2539 
2540  ActionPrimitivesWayPoint wp;
2541  wp.x=deployPrepare; wp.o=reachAboveCata[arm];
2542  wp.oEnabled=true; wp.duration=1.0;
2543  deque<ActionPrimitivesWayPoint> wpList;
2544  wpList.push_back(wp);
2545  wp.x=deployEnd; wp.duration=3.0;
2546  wpList.push_back(wp);
2547  action[arm]->enableReachingTimeout(3.0*reachingTimeout);
2548  action[arm]->pushAction(wpList);
2549 
2550  action[arm]->checkActionsDone(f,true);
2551  action[arm]->pushWaitState(1.0);
2552  if (auto* p = dynamic_cast<ActionPrimitivesLayer2*>(action[arm]))
2553  p->disableContactDetection();
2554  action[arm]->enableReachingTimeout(reachingTimeout);
2555 
2556  bool found;
2557  if (auto* p = dynamic_cast<ActionPrimitivesLayer2*>(action[arm]))
2558  p->checkContact(found);
2559 
2560  if(found)
2561  {
2562  Vector x,o;
2563  action[arm]->getPose(x,o);
2564 
2565  if(!checkOptions(options,"no_head") && !checkOptions(options,"no_gaze"))
2566  ctrl_gaze->lookAtFixationPoint(x);
2567 
2568  table_height=x[2];
2569  table[3]=-table_height;
2570 
2571  bKinOffsets.find("table_height")=Value(table_height);
2572 
2573  //save the table height on file
2574  saveKinematicOffsets();
2575 
2576  //save the table height also in the object database
2577  opcPort.setTableHeight(table_height);
2578 
2579  yWarning("########## Table height found: %f",table_height);
2580 
2581  //adjust the table height accordingly to a specified tolerance
2582  table_height+=table_height_tolerance;
2583  }
2584  else
2585  yWarning("########## Table height not found!");
2586 
2587  if(!checkOptions(options,"no_head") && !checkOptions(options,"no_gaze"))
2588  setGazeIdle();
2589 
2590  goHome(options);
2591 
2592  return found;
2593 }
2594 
2595 
2596 bool MotorThread::calibFingers(Bottle &options)
2597 {
2598  int currentArm=armInUse;
2599 
2600  int handToCalibrate=-1;
2601  if(checkOptions(options,"left") || checkOptions(options,"right"))
2602  handToCalibrate=checkOptions(options,"left")?LEFT:RIGHT;
2603 
2604  bool no_head=(checkOptions(options,"no_head"));
2605 
2606  bool calibrated=true;
2607  for(int arm=0; arm<2; arm++)
2608  {
2609  if(action[arm]!=NULL && (handToCalibrate==-1 || handToCalibrate==arm))
2610  {
2611  setArmInUse(arm);
2612 
2613  if(!no_head)
2614  lookAtHand(options);
2615 
2616  Bottle b(arm==LEFT?"left":"right");
2617  drawNear(b);
2618 
2619  Bottle fingers;
2620  Bottle &fng=fingers.addList();
2621  fng.addString("index");
2622  fng.addString("middle");
2623  fng.addString("ring");
2624  fng.addString("little");
2625 
2626  Property prop;
2627  prop.put("finger",fingers.get(0));
2628  graspModel[arm]->calibrate(prop);
2629 
2630  prop.clear();
2631  prop.put("finger","thumb");
2632  graspModel[arm]->calibrate(prop);
2633 
2634  ofstream fout;
2635  fout.open((rf.getHomeContextPath()+"/"+graspFile[arm]).c_str());
2636  graspModel[arm]->toStream(fout);
2637  fout.close();
2638 
2639  calibrated=calibrated && graspModel[arm]->isCalibrated();
2640 
2641  bool f;
2642  action[arm]->pushAction(homePos[arm],homeOrient[arm],"open_hand");
2643  action[arm]->checkActionsDone(f,true);
2644  }
2645  }
2646 
2647  if (!no_head)
2648  {
2649  Bottle options;
2650  options.addString("head");
2651  goHome(options);
2652  }
2653 
2654  setArmInUse(currentArm);
2655 
2656  return calibrated;
2657 }
2658 
2659 
2660 bool MotorThread::avoidTable(bool avoid)
2661 {
2662  for(int arm=0; arm<2; arm++)
2663  {
2664  if(action[arm]!=NULL)
2665  {
2666  action[arm]->setTrackingMode(false);
2667 
2668  Vector x,o;
2669  action[arm]->getPose(x,o);
2670  avoid_table_height[arm]=x[2];
2671  }
2672  }
2673 
2674  avoid_table=avoid;
2675  return true;
2676 }
2677 
2678 
2679 bool MotorThread::exploreTorso(Bottle &options)
2680 {
2681  int dbg_cnt=0;
2682  this->avoidTable(true);
2683 
2684  Bottle info;
2685  ctrl_gaze->getInfo(info);
2686  iKinLimbVersion head_version(info.check("head_version",Value("1.0")).asString());
2687 
2688  ostringstream type;
2689  type<<(dominant_eye==LEFT?"left":"right");
2690  if (head_version==iKinLimbVersion("2.0"))
2691  type<<"_v2";
2692 
2693  iCubEye iKinTorso=iCubEye(type.str());
2694  iKinTorso.releaseLink(0); // pitch
2695  iKinTorso.releaseLink(1); // roll
2696  for (unsigned int i=2; i<iKinTorso.getN(); i++)
2697  iKinTorso.blockLink(i,0.0);
2698 
2699  //get the torso initial position
2700  Vector torso_init_joints(3);
2701  enc_torso->getEncoders(torso_init_joints.data());
2702 
2703  //initialization for the random walker
2704  //the iKinTorso needs the {0,2} joints switched
2705  Vector tmp_joints(2,0.0);
2706  tmp_joints[0]=CTRL_DEG2RAD*torso_init_joints[2];
2707  tmp_joints[1]=CTRL_DEG2RAD*torso_init_joints[1];
2708  iKinTorso.setAng(tmp_joints);
2709  iKinTorso.setBlockingValue(2,CTRL_DEG2RAD*torso_init_joints[0]);
2710 
2711  Matrix H=iKinTorso.getH(3,true);
2712  Vector cart_init_pos(3);
2713  cart_init_pos[0]=H[0][3];
2714  cart_init_pos[1]=H[1][3];
2715  cart_init_pos[2]=H[2][3];
2716  //----------------
2717 
2718  //fixed "target" position
2719  Vector fixed_target(3);
2720  fixed_target[0]=-0.5;
2721  fixed_target[1]=0.0;
2722  fixed_target[2]=cart_init_pos[2];
2723 
2724  double walking_time=20.0;
2725  double step_time=2.0;
2726  double kp_pos_torso=0.6;
2727 
2728  vector<int> modes(3,VOCAB_CM_VELOCITY);
2729  ctrl_mode_torso->setControlModes(modes.data());
2730 
2731  double init_walking_time=Time::now();
2732 
2733  //random walk!
2734  while(this->isRunning() && !interrupted && Time::now()-init_walking_time<walking_time)
2735  {
2736  //generate next random step
2737  Vector random_pos=cart_init_pos;
2738  if (Rand::scalar(0.0,3.0)>2.0) // 1/3 => lean forward
2739  random_pos[0]+=Rand::scalar(-0.1,-0.05);
2740  else // 2/3 => move sideways
2741  {
2742  double tmp_rnd=Rand::scalar(-random_pos_y,random_pos_y);
2743  if ((tmp_rnd>-0.1) && (tmp_rnd<0.1))
2744  tmp_rnd=0.1*yarp::math::sign(tmp_rnd);
2745 
2746  random_pos[1]+=tmp_rnd;
2747  }
2748 
2749  random_pos=(norm(cart_init_pos)/norm(random_pos))*random_pos;
2750 
2751  //wait to reach that point
2752  double init_step_time=Time::now();
2753  while(this->isRunning() && !interrupted && Time::now()-init_step_time<step_time && Time::now()-init_walking_time<walking_time)
2754  {
2755  //set the current torso joints to the iKinTorso
2756  Vector torso_joints(3);
2757  enc_torso->getEncoders(torso_joints.data());
2758 
2759  Vector tmp_joints(2,0.0);
2760  tmp_joints[0]=CTRL_DEG2RAD*torso_joints[2];
2761  tmp_joints[1]=CTRL_DEG2RAD*torso_joints[1];
2762  iKinTorso.setAng(tmp_joints);
2763  iKinTorso.setBlockingValue(2,CTRL_DEG2RAD*torso_joints[0]);
2764 
2765  Matrix H=iKinTorso.getH(3,true);
2766  Vector curr_pos(3);
2767  curr_pos[0]=H[0][3];
2768  curr_pos[1]=H[1][3];
2769  curr_pos[2]=H[2][3];
2770 
2771  Vector x_dot=kp_pos_torso*(random_pos-curr_pos);
2772  Matrix J=iKinTorso.GeoJacobian(3).submatrix(0,2,0,1);
2773  Matrix J_pinv=pinv(J,1e-06);
2774  Vector q_dot=CTRL_RAD2DEG*(J_pinv*x_dot);
2775  double q_dot_mag=norm(q_dot);
2776  double q_dot_saturation=15.0;
2777 
2778  if (q_dot_mag<1.0)
2779  {
2780  vel_torso->stop();
2781  break;
2782  }
2783  else if (q_dot_mag>q_dot_saturation)
2784  q_dot=(q_dot_saturation/q_dot_mag)*q_dot;
2785 
2786  // account for specific order
2787  vector<int> jnts(2);
2788  jnts[0]=2;
2789  jnts[1]=1;
2790  vel_torso->velocityMove((int)jnts.size(),jnts.data(),q_dot.data());
2791  Time::delay(0.01);
2792  }
2793  }
2794 
2795  //go back to torso initial position
2796  modes[0]=modes[1]=modes[2]=VOCAB_CM_POSITION;
2797  ctrl_mode_torso->setControlModes(modes.data());
2798  pos_torso->positionMove(torso_init_joints.data());
2799  bool done=false;
2800  while (isRunning() && !done)
2801  {
2802  Time::delay(0.1);
2803  pos_torso->checkMotionDone(&done);
2804  }
2805 
2806  this->avoidTable(false);
2807  return true;
2808 }
2809 
2810 
2811 bool MotorThread::exploreHand(Bottle &options)
2812 {
2813  if(arm_mode!=ARM_MODE_IDLE)
2814  {
2815  yError("Error! The requested arm is busy!");
2816  return false;
2817  }
2818 
2819  int arm=ARM_IN_USE;
2820  if(checkOptions(options,"left") || checkOptions(options,"right"))
2821  arm=checkOptions(options,"left")?LEFT:RIGHT;
2822 
2823  arm=checkArm(arm);
2824 
2825  if(action[arm]==NULL)
2826  {
2827  yError("Error! requested arm is not working!");
2828  return false;
2829  }
2830 
2831  //if it was not specified by the user to keep the other hand still, bring it back home
2832  int other_arm=1-arm;
2833  if(!checkOptions(options,"keep_other_hand_still") && action[other_arm]!=NULL)
2834  {
2835  action[other_arm]->pushAction(homePos[other_arm],homeOrient[other_arm]);
2836 
2837  bool f;
2838  action[other_arm]->checkActionsDone(f,true);
2839  }
2840 
2841  //completely disable the arm control
2842  Bottle bInterrupt("skip");
2843  suspendLearningModeAction(bInterrupt);
2844  suspendLearningModeKinOffset(bInterrupt);
2845 
2846  action[arm]->lockActions();
2847  action[arm]->syncCheckInterrupt(true);
2848  action[arm]->stopControl();
2849  //-----------------------------------
2850 
2851  if(!checkOptions(options,"no_head") && !checkOptions(options,"no_gaze"))
2852  {
2853  setGazeIdle();
2854 
2855  lookAtHand(options);
2856  }
2857 
2858  double max_step_time=2.0;
2859 
2860  int nJnts;
2861  enc_arm[arm]->getAxes(&nJnts);
2862 
2863  vector<int> modes(nJnts,VOCAB_CM_POSITION);
2864  ctrl_mode_arm[arm]->setControlModes(modes.data());
2865 
2866  //start exploration
2867  Vector destination(nJnts);
2868  for(unsigned int pose_idx=0; pose_idx<handPoses.size(); pose_idx++)
2869  {
2870  Vector current_position(nJnts);
2871  enc_arm[arm]->getEncoders(current_position.data());
2872 
2873  //copy the arm configuration in the destination vector
2874  for(unsigned int i=0; i<handPoses[pose_idx].size(); i++)
2875  destination[i]=handPoses[pose_idx][i];
2876 
2877  //do not change the fingers angles
2878  for(int i=handPoses[pose_idx].size(); i<nJnts; i++)
2879  destination[i]=current_position[i];
2880 
2881  pos_arm[arm]->positionMove(destination.data());
2882 
2883  double init_step_time=Time::now();
2884  while(!this->interrupted && Time::now()-init_step_time<max_step_time)
2885  {
2886  enc_arm[arm]->getEncoders(current_position.data());
2887 
2888  bool done=true;
2889  for(size_t i=0; i<handPoses[pose_idx].size(); i++)
2890  if(fabs(destination[i]-current_position[i])>3.0)
2891  done=false;
2892 
2893  if(done)
2894  break;
2895 
2896  Time::delay(0.05);
2897  }
2898  }
2899 
2900  if (!checkOptions(options,"no_head") && !checkOptions(options,"no_gaze"))
2901  setGazeIdle();
2902 
2903  //re-enable the arm control
2904  if(action[arm]!=NULL)
2905  {
2906  action[arm]->unlockActions();
2907  action[arm]->syncCheckReinstate();
2908  }
2909 
2910  return true;
2911 }
2912 
2913 
2914 bool MotorThread::startLearningModeAction(Bottle &options)
2915 {
2916  if(arm_mode!=ARM_MODE_IDLE)
2917  {
2918  yError("Error: The requested arm is busy!");
2919  return false;
2920  }
2921 
2922  string action_name=options.find("action_name").asString();
2923 
2924  if(action_name=="")
2925  {
2926  yError("action name not specified!");
2927  return false;
2928  }
2929 
2930  int arm=ARM_IN_USE;
2931  if(checkOptions(options,"left") || checkOptions(options,"right"))
2932  arm=checkOptions(options,"left")?LEFT:RIGHT;
2933 
2934  arm=checkArm(arm);
2935 
2936  string arm_name=(arm==LEFT?"left":"right");
2937 
2938  string fileName=rf.findFile(actions_path+"/"+arm_name+"/"+action_name+".action");
2939  if(!fileName.empty())
2940  {
2941  yWarning("Action '%s' already learned... stopping",action_name.c_str());
2942  return false;
2943  }
2944 
2945  dragger.actionName=action_name;
2946  dragger.actions.clear();
2947 
2948  bool f;
2949  action[arm]->checkActionsDone(f,true);
2950  action[arm]->getCartesianIF(dragger.ctrl);
2951 
2952  if(dragger.ctrl==NULL)
2953  {
2954  yError("Could not find the cartesian arm interface!");
2955  return false;
2956  }
2957 
2958  dragger.arm=arm;
2959 
2960  Vector x,o;
2961  dragger.ctrl->getPose(x,o);
2962  dragger.x0=x;
2963 
2964  if(!setTorque(true,arm))
2965  {
2966  yError("Could not set torque control mode!");
2967  return false;
2968  }
2969 
2970  dragger.t0=Time::now();
2971  arm_mode=ARM_MODE_LEARN_ACTION;
2972 
2973  return true;
2974 }
2975 
2976 
2977 bool MotorThread::suspendLearningModeAction(Bottle &options)
2978 {
2979  if(arm_mode!=ARM_MODE_LEARN_ACTION)
2980  return false;
2981 
2982  string arm_name=(dragger.arm==LEFT?"left":"right");
2983 
2984  bool success=true;
2985  bool skip=checkOptions(options,"skip");
2986 
2987  if(!skip)
2988  {
2989  if (!actions_path.empty())
2990  {
2991  string fileName=rf.getHomeContextPath();
2992  fileName+="/"+actions_path+"/"+arm_name+"/"+dragger.actionName+".action";
2993  ofstream action_fout(fileName.c_str());
2994  if(!action_fout.is_open())
2995  {
2996  yError("Unable to open file '%s' for action %s!",(actions_path+"/"+arm_name+"/"+dragger.actionName+".action").c_str(),dragger.actionName.c_str());
2997  success=false;
2998  }
2999  else
3000  action_fout << dragger.actions.toString();
3001  }
3002  else
3003  success = opcPort.setAction(dragger.actionName, &(dragger.actions));
3004  }
3005 
3006  // set back again to (impedance) velocity mode
3007  arm_mode=ARM_MODE_IDLE;
3008  Time::delay(0.1);
3009 
3010  setTorque(false);
3011 
3012  dragger.ctrl=NULL;
3013 
3014  return success;
3015 }
3016 
3017 
3018 bool MotorThread::imitateAction(Bottle &options)
3019 {
3020  int arm=ARM_IN_USE;
3021  if(checkOptions(options,"left") || checkOptions(options,"right"))
3022  arm=checkOptions(options,"left")?LEFT:RIGHT;
3023 
3024  arm=checkArm(arm);
3025 
3026  string arm_name=arm==LEFT?"left":"right";
3027  string action_name=options.find("action_name").asString();
3028 
3029  Bottle actions;
3030  string fileName=rf.findFile(actions_path+"/"+arm_name+"/"+action_name+".action");
3031  if (!fileName.empty())
3032  {
3033  ifstream action_fin(fileName.c_str());
3034  if(!action_fin.is_open())
3035  return false;
3036 
3037  stringstream strstr;
3038  strstr << action_fin.rdbuf();
3039  actions.fromString(strstr.str());
3040  }
3041  else if (!opcPort.getAction(action_name, &actions))
3042  return false;
3043 
3044  restoreContext(arm);
3045 
3046  ICartesianControl *ctrl;
3047  action[arm]->getCartesianIF(ctrl);
3048 
3049  Vector newPos;
3050  ctrl->getDOF(newPos);
3051  newPos[0]=newPos[1]=0.0; newPos[2]=1.0;
3052  ctrl->setDOF(newPos,newPos);
3053  ctrl->setInTargetTol(0.01);
3054  ctrl->setTrajTime(0.75);
3055 
3056  Vector init_x,init_o;
3057  ctrl->getPose(init_x,init_o);
3058 
3059  for(int i=0; i<actions.size(); i++)
3060  {
3061  Bottle *b=actions.get(i).asList();
3062  Vector x(3),o(4);
3063  for(int j=0; j<3; j++)
3064  x[j]=b->get(j).asFloat64();
3065 
3066  for(int j=0; j<4; j++)
3067  o[j]=b->get(j+3).asFloat64();
3068 
3069  ctrl->goToPoseSync(init_x-x,o);
3070  Time::delay(0.1);
3071  }
3072 
3073  ctrl->waitMotionDone(0.1,reachingTimeout);
3074  restoreContext(arm);
3075 
3076  return true;
3077 }
3078 
3079 
3080 bool MotorThread::startLearningModeKinOffset(Bottle &options)
3081 {
3082  //check if kinematic offset learning is already going on and if the system is not busy
3083  if(dragger.ctrl!=NULL || arm_mode!=ARM_MODE_IDLE)
3084  return false;
3085 
3086  int arm=ARM_IN_USE;
3087  if(checkOptions(options,"left") || checkOptions(options,"right"))
3088  arm=checkOptions(options,"left")?LEFT:RIGHT;
3089 
3090  dragger.arm=checkArm(arm);
3091 
3092  bool f;
3093  action[dragger.arm]->checkActionsDone(f,true);
3094 
3095  action[dragger.arm]->getCartesianIF(dragger.ctrl);
3096  if(dragger.ctrl==NULL)
3097  {
3098  yError("Could not find the arm controller!");
3099  return false;
3100  }
3101 
3102  //if the impedance control is available use it otherwise employ admittance control
3103  dragger.using_impedance=setTorque(true,dragger.arm);
3104  if (!dragger.using_impedance)
3105  yWarning("Impedance control not available. Using admittance control!");
3106 
3107  Vector x(3,0.0);
3108  bool specifiedTarget=false;
3109  if (Bottle *b0=options.find("target").asList())
3110  {
3111  if (Bottle *b1=b0->find("cartesian").asList())
3112  {
3113  size_t n=std::min(x.length(),(size_t)b1->size());
3114  for (size_t i=0; i<n; i++)
3115  x[i]=b1->get(i).asFloat64();
3116  specifiedTarget=true;
3117  }
3118  }
3119 
3120  if (!specifiedTarget)
3121  {
3122  Vector o;
3123  dragger.ctrl->getPose(x,o);
3124  }
3125 
3126  dragger.x0=x;
3127  dragger.t0=Time::now();
3128  yInfo("Learning kinematic offset against %s target (%s)",
3129  specifiedTarget?"specified":"retrieved",
3130  dragger.x0.toString(3,3).c_str());
3131 
3132  arm_mode=ARM_MODE_LEARN_KINOFF;
3133 
3134  Bottle look_options;
3135  look_options.addString("hand");
3136  look_options.addString(dragger.arm==LEFT?"left":"right");
3137  look(look_options);
3138 
3139  return true;
3140 }
3141 
3142 
3143 bool MotorThread::suspendLearningModeKinOffset(Bottle &options)
3144 {
3145  if(arm_mode!=ARM_MODE_LEARN_KINOFF)
3146  return false;
3147 
3148  bool skip=checkOptions(options,"skip");
3149 
3150  if(!skip)
3151  {
3152  //compute the offset
3153  Vector x,o;
3154  dragger.ctrl->getPose(x,o);
3155 
3156  if (options.size()>=4)
3157  opcPort.getKinematicOffsets(options.get(3).asString(),currentKinematicOffset);
3158  currentKinematicOffset[dragger.arm]=x-(dragger.x0-currentKinematicOffset[dragger.arm]);
3159 
3160  //if no object with specified name is present in the database, then update the default kinematic offsets
3161  if(options.size()<4 || !opcPort.setKinematicOffsets(options.get(3).asString(),currentKinematicOffset))
3162  {
3163  defaultKinematicOffset[dragger.arm]=currentKinematicOffset[dragger.arm];
3164 
3165  //update the offset and save on file
3166  Bottle bOffset;
3167  for(int i=0; i<3; i++)
3168  bOffset.addFloat64(defaultKinematicOffset[dragger.arm][i]);
3169 
3170  string arm_name=(dragger.arm==LEFT?"left":"right");
3171  *bKinOffsets.find(arm_name).asList()->find(Vocab32::decode(modeS2C)).asList()=bOffset;
3172 
3173  saveKinematicOffsets();
3174  }
3175  }
3176 
3177  arm_mode=ARM_MODE_IDLE;
3178  Time::delay(0.1);
3179 
3180  dragger.ctrl=NULL;
3181 
3182  setTorque(false);
3183  setGazeIdle();
3184 
3185  return true;
3186 }
3187 
3188 
3189 void MotorThread::getStatus(Bottle &status)
3190 {
3191  Bottle &gaze=status.addList();
3192  gaze.addString("gaze");
3193 
3194  if(head_mode!=HEAD_MODE_IDLE)
3195  gaze.addString("busy");
3196  else
3197  gaze.addString("idle");
3198 
3199  Bottle &control_mode=status.addList();
3200  control_mode.addString("control mode");
3201  control_mode.addString(status_impedance_on?"impedance":"velocity");
3202 
3203  Bottle &left_arm=status.addList();
3204  left_arm.addString("left_arm");
3205 
3206  if(action[LEFT]!=NULL)
3207  {
3208  bool ongoing;
3209  action[LEFT]->checkActionsDone(ongoing,false);
3210 
3211  if(!ongoing)
3212  left_arm.addString("busy");
3213  else
3214  left_arm.addString("idle");
3215  }
3216  else
3217  left_arm.addString("unavailable");
3218 
3219 
3220  Bottle &right_arm=status.addList();
3221  right_arm.addString("right_arm");
3222 
3223  if(action[RIGHT]!=NULL)
3224  {
3225  bool ongoing;
3226  action[RIGHT]->checkActionsDone(ongoing,false);
3227 
3228  if(!ongoing)
3229  right_arm.addString("busy");
3230  else
3231  right_arm.addString("idle");
3232  }
3233  else
3234  right_arm.addString("unavailable");
3235 
3236 
3237 
3238  Bottle &statusS2C=status.addList();
3239  statusS2C.addString("[Stereo -> Cartesian] mode: ");
3240 
3241  switch(modeS2C)
3242  {
3243  case S2C_HOMOGRAPHY:
3244  {
3245  statusS2C.addString("homography");
3246  break;
3247  }
3248 
3249  case S2C_DISPARITY:
3250  {
3251  statusS2C.addString("disparity");
3252  break;
3253  }
3254 
3255  case S2C_NETWORK:
3256  {
3257  statusS2C.addString("network");
3258  break;
3259  }
3260  }
3261 }
3262 
3263 
3264 void MotorThread::update()
3265 {
3266  if(opcPort.isUpdateNeeded())
3267  {
3268  opcPort.getTableHeight(table_height);
3269 
3270  table.resize(4,0.0);
3271  table[2]=1.0;
3272  table[3]=-table_height;
3273 
3274  //adjust the table height accordingly to a specified tolerance
3275  table_height+=table_height_tolerance;
3276  }
3277 }
3278 
3279 
3280 void MotorThread::interrupt()
3281 {
3282  interrupted=true;
3283 
3284  disparityPort.interrupt();
3285 
3286  //if learning is going on
3287  Bottle bInterrupt("skip");
3288  suspendLearningModeAction(bInterrupt);
3289  suspendLearningModeKinOffset(bInterrupt);
3290 
3291  if (ctrl_gaze!=NULL)
3292  {
3293  setGazeIdle();
3294  ctrl_gaze->stopControl();
3295  }
3296 
3297  if(action[LEFT]!=NULL)
3298  {
3299  action[LEFT]->lockActions();
3300  action[LEFT]->syncCheckInterrupt(true);
3301  action[LEFT]->stopControl();
3302  }
3303 
3304  if(action[RIGHT]!=NULL)
3305  {
3306  action[RIGHT]->lockActions();
3307  action[RIGHT]->syncCheckInterrupt(true);
3308  action[RIGHT]->stopControl();
3309  }
3310 }
3311 
3312 
3313 void MotorThread::reinstate()
3314 {
3315  if(action[LEFT]!=NULL)
3316  {
3317  action[LEFT]->unlockActions();
3318  action[LEFT]->syncCheckReinstate();
3319  }
3320 
3321  if(action[RIGHT]!=NULL)
3322  {
3323  action[RIGHT]->unlockActions();
3324  action[RIGHT]->syncCheckReinstate();
3325  }
3326 
3327  disparityPort.resume();
3328 
3329  interrupted=false;
3330 }
3331 
3332 
HEAD_MODE_TRACK_TEMP
#define HEAD_MODE_TRACK_TEMP
Definition: MotorThread.h:42
S2C_HOMOGRAPHY
#define S2C_HOMOGRAPHY
Definition: MotorThread.h:58
HEAD_MODE_IDLE
#define HEAD_MODE_IDLE
Definition: MotorThread.h:39
GRASP_STATE_IDLE
#define GRASP_STATE_IDLE
Definition: MotorThread.h:51
ARM_MODE_LEARN_ACTION
#define ARM_MODE_LEARN_ACTION
Definition: MotorThread.h:47
HEAD_MODE_TRACK_HAND
#define HEAD_MODE_TRACK_HAND
Definition: MotorThread.h:41
HEAD_MODE_TRACK_CART
#define HEAD_MODE_TRACK_CART
Definition: MotorThread.h:43
S2C_DISPARITY
#define S2C_DISPARITY
Definition: MotorThread.h:59
ARM_MODE_LEARN_KINOFF
#define ARM_MODE_LEARN_KINOFF
Definition: MotorThread.h:48
S2C_NETWORK
#define S2C_NETWORK
Definition: MotorThread.h:60
ARM_MODE_FINE_REACHING
#define ARM_MODE_FINE_REACHING
Definition: MotorThread.h:49
ARM_MODE_IDLE
#define ARM_MODE_IDLE
Definition: MotorThread.h:46
GRASP_STATE_SIDE
#define GRASP_STATE_SIDE
Definition: MotorThread.h:53
HEAD_MODE_TRACK_FIX
#define HEAD_MODE_TRACK_FIX
Definition: MotorThread.h:44
MotorThread::push
bool push(Bottle &options)
Definition: MotorThread.cpp:1743
MotorThread::targetToCartesian
bool targetToCartesian(Bottle *target, Vector &xd)
Definition: MotorThread.cpp:339
MotorThread::calibTable
bool calibTable(Bottle &options)
Definition: MotorThread.cpp:2505
MotorThread::powerGrasp
bool powerGrasp(Bottle &options)
Definition: MotorThread.cpp:1677
MotorThread::startLearningModeAction
bool startLearningModeAction(Bottle &options)
Definition: MotorThread.cpp:2914
MotorThread::threadInit
virtual bool threadInit()
Definition: MotorThread.cpp:926
MotorThread::reach
bool reach(Bottle &options)
Definition: MotorThread.cpp:1584
MotorThread::grasp
bool grasp(const Bottle &options)
Definition: MotorThread.cpp:2123
MotorThread::drawNear
bool drawNear(Bottle &options)
Definition: MotorThread.cpp:2420
MotorThread::isHolding
bool isHolding(const Bottle &options)
Definition: MotorThread.cpp:2489
MotorThread::stereoToCartesian
bool stereoToCartesian(const Vector &stereo, Vector &xd)
Definition: MotorThread.cpp:383
MotorThread::clearIt
bool clearIt(Bottle &options)
Definition: MotorThread.cpp:2077
MotorThread::getStatus
void getStatus(Bottle &status)
Definition: MotorThread.cpp:3189
MotorThread::point
bool point(Bottle &options)
Definition: MotorThread.cpp:1796
MotorThread::interrupt
void interrupt()
Definition: MotorThread.cpp:3280
MotorThread::startLearningModeKinOffset
bool startLearningModeKinOffset(Bottle &options)
Definition: MotorThread.cpp:3080
MotorThread::hand
bool hand(const Bottle &options, const string &type="", bool *holding=NULL)
Definition: MotorThread.cpp:2085
MotorThread::give
bool give(Bottle &options)
Definition: MotorThread.cpp:2033
MotorThread::onStop
virtual void onStop()
Definition: MotorThread.cpp:1552
MotorThread::point_far
bool point_far(Bottle &options)
Definition: MotorThread.cpp:1841
MotorThread::imitateAction
bool imitateAction(Bottle &options)
Definition: MotorThread.cpp:3018
MotorThread::exploreHand
bool exploreHand(Bottle &options)
Definition: MotorThread.cpp:2811
MotorThread::threadRelease
virtual void threadRelease()
Definition: MotorThread.cpp:1546
MotorThread::look
bool look(Bottle &options)
Definition: MotorThread.cpp:1904
MotorThread::deploy
bool deploy(Bottle &options)
Definition: MotorThread.cpp:2341
MotorThread::run
virtual void run()
Definition: MotorThread.cpp:1348
MotorThread::suspendLearningModeKinOffset
bool suspendLearningModeKinOffset(Bottle &options)
Definition: MotorThread.cpp:3143
MotorThread::exploreTorso
bool exploreTorso(Bottle &options)
Definition: MotorThread.cpp:2679
MotorThread::changeElbowHeight
bool changeElbowHeight(const int arm, const double height, const double weight)
Definition: MotorThread.cpp:2140
MotorThread::suspendLearningModeAction
bool suspendLearningModeAction(Bottle &options)
Definition: MotorThread.cpp:2977
MotorThread::reinstate
void reinstate()
Definition: MotorThread.cpp:3313
MotorThread::setTorque
bool setTorque(bool turn_on, int arm=ARM_IN_USE)
Definition: MotorThread.cpp:225
MotorThread::setImpedance
bool setImpedance(bool turn_on)
Definition: MotorThread.cpp:203
MotorThread::changeExecTime
bool changeExecTime(const int arm, const double execTime)
Definition: MotorThread.cpp:2182
MotorThread::release
bool release(const Bottle &options)
Definition: MotorThread.cpp:2133
MotorThread::takeTool
bool takeTool(Bottle &options)
Definition: MotorThread.cpp:1943
MotorThread::calibFingers
bool calibFingers(Bottle &options)
Definition: MotorThread.cpp:2596
MotorThread::shiftAndGrasp
bool shiftAndGrasp(Bottle &options)
Definition: MotorThread.cpp:2443
MotorThread::getHandImagePosition
bool getHandImagePosition(Bottle &hand_image_pos)
Definition: MotorThread.cpp:2466
MotorThread::goHome
bool goHome(Bottle &options)
Definition: MotorThread.cpp:2249
MotorThread::setStereoToCartesianMode
int setStereoToCartesianMode(const int mode, Bottle &reply)
Definition: MotorThread.cpp:255
MotorThread::goUp
bool goUp(Bottle &options, const double h=std::numeric_limits< double >::quiet_NaN())
Definition: MotorThread.cpp:1562
MotorThread::expect
bool expect(Bottle &options)
Definition: MotorThread.cpp:1988
iCub::action::ActionPrimitivesLayer1
A derived class defining a first abstraction layer on top of actionPrimitives father class.
Definition: actionPrimitives.h:931
iCub::action::ActionPrimitivesLayer2
A class that inherits from ActionPrimitivesLayer1 and integrates the force-torque sensing in order to...
Definition: actionPrimitives.h:1070
iCub::action::ActionPrimitives
The base class defining actions.
Definition: actionPrimitives.h:194
iCub::action::ActionPrimitives::areFingersInPosition
virtual bool areFingersInPosition(bool &f)
Query if fingers are in position (cumulative response).
Definition: actionPrimitives.cpp:1767
iCub::action::ActionPrimitives::lockActions
virtual bool lockActions()
Disable the possibility to yield any new action.
Definition: actionPrimitives.cpp:839
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:2043
iCub::action::ActionPrimitives::enableReachingTimeout
virtual bool enableReachingTimeout(const double tmo)
Enable timeout while reaching.
Definition: actionPrimitives.cpp:1975
iCub::action::ActionPrimitives::setTrackingMode
virtual bool setTrackingMode(const bool f)
Set the task space controller in tracking or non-tracking mode.
Definition: actionPrimitives.cpp:1916
iCub::action::ActionPrimitives::unlockActions
virtual bool unlockActions()
Enable the possibility to yield new actions.
Definition: actionPrimitives.cpp:852
iCub::action::ActionPrimitives::getPose
virtual bool getPose(yarp::sig::Vector &x, yarp::sig::Vector &o) const
Get the current arm pose.
Definition: actionPrimitives.cpp:1850
iCub::action::ActionPrimitives::close
virtual void close()
Deallocate the object.
Definition: actionPrimitives.cpp:703
iCub::action::ActionPrimitives::getHandSequence
virtual bool getHandSequence(const std::string &handSeqKey, yarp::os::Bottle &sequence)
Return a hand sequence.
Definition: actionPrimitives.cpp:1690
iCub::action::ActionPrimitives::stopControl
virtual bool stopControl()
Stop any ongoing arm/hand movements.
Definition: actionPrimitives.cpp:1863
iCub::action::ActionPrimitives::setDefaultExecTime
virtual bool setDefaultExecTime(const double execTime)
Set the default arm movement execution time.
Definition: actionPrimitives.cpp:1896
iCub::action::ActionPrimitives::getGraspModel
virtual bool getGraspModel(perception::Model *&model) const
Return the model used internally to detect external contacts.
Definition: actionPrimitives.cpp:1795
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:1808
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::getHandSeqList
std::deque< std::string > getHandSeqList()
Return the list of available hand sequence keys.
Definition: actionPrimitives.cpp:1677
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:1941
iCub::action::ActionPrimitives::syncCheckReinstate
virtual bool syncCheckReinstate()
Reinstate the blocking feature for future calls with sync switch on.
Definition: actionPrimitives.cpp:2060
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:2003
iCub::action::ActionPrimitives::pushWaitState
virtual bool pushWaitState(const double tmo, ActionPrimitivesCallback *clb=NULL)
Insert a wait state in the actions queue.
Definition: actionPrimitives.cpp:1125
iCub::ctrl::Integrator
A class for defining a saturated integrator based on Tustin formula: .
Definition: pids.h:48
iCub::iKin::iCubEye
A class for defining the iCub Eye.
Definition: iKinFwd.h:1386
iCub::iKin::iKinChain::getH
yarp::sig::Matrix getH(const unsigned int i, const bool allLink=false)
Returns the rigid roto-translation matrix from the root reference frame to the ith frame in Denavit-H...
Definition: iKinFwd.cpp:732
iCub::iKin::iKinChain::GeoJacobian
yarp::sig::Matrix GeoJacobian(const unsigned int i)
Returns the geometric Jacobian of the ith link.
Definition: iKinFwd.cpp:1012
iCub::iKin::iKinChain::releaseLink
bool releaseLink(const unsigned int i)
Releases the ith Link.
Definition: iKinFwd.cpp:463
iCub::iKin::iKinChain::setBlockingValue
bool setBlockingValue(const unsigned int i, double Ang)
Changes the value of the ith blocked Link.
Definition: iKinFwd.cpp:414
iCub::iKin::iKinChain::setAng
yarp::sig::Vector setAng(const yarp::sig::Vector &q)
Sets the free joint angles to values of q[i].
iCub::iKin::iKinChain::getN
unsigned int getN() const
Returns the number of Links belonging to the Chain.
Definition: iKinFwd.h:550
iCub::iKin::iKinChain::blockLink
bool blockLink(const unsigned int i, double Ang)
Blocks the ith Link at the a certain value of its joint angle.
Definition: iKinFwd.cpp:394
iCub::iKin::iKinLimbVersion
A class for defining the versions of the iCub limbs.
Definition: iKinFwd.h:1046
e
e
Definition: compute_ekf_fast.m:13
H
H
Definition: compute_ekf_fast.m:27
zeros
zeros(2, 2) eye(2
h
h
Definition: compute_ekf_sym.m:23
f
f
Definition: compute_ekf_sym.m:22
x
x
Definition: compute_ekf_sym.m:21
T
T
Definition: compute_ekf_sym.m:12
n
int n
Definition: debugFunctions.cpp:58
iCub::ctrl::norm
double norm(const yarp::sig::Matrix &M, int col)
Returns the norm of the vector given in the form: matrix(:,col).
done
bool done
Definition: main.cpp:42
jnts
int jnts
Definition: main.cpp:60
LEFT
#define LEFT
Definition: utils.h:33
ARM_IN_USE
#define ARM_IN_USE
Definition: utils.h:35
RIGHT
#define RIGHT
Definition: utils.h:34
ARM_MOST_SUITED
#define ARM_MOST_SUITED
Definition: utils.h:36
iCub::ctrl::CTRL_RAD2DEG
constexpr double CTRL_RAD2DEG
180/PI.
Definition: math.h:61
iCub::ctrl::CTRL_DEG2RAD
constexpr double CTRL_DEG2RAD
PI/180.
Definition: math.h:66
strain::dsp::fsc::max
const FSC max
Definition: strain.h:48
strain::dsp::fsc::min
const FSC min
Definition: strain.h:49
pointing_far.h
y
y
Definition: show_eyes_axes.m:21
p
p
Definition: show_eyes_axes.m:23
out
out
Definition: sine.m:8
offset
degrees offset
Definition: sine.m:4
Dragger::init
void init(Bottle &initializer, double thread_rate)
Definition: MotorThread.cpp:58
PointingFar::compute
static bool compute(yarp::dev::ICartesianControl *iarm, const yarp::os::Property &requirements, yarp::sig::Vector &q, yarp::sig::Vector &x)
Definition: pointing_far.cpp:300
PointingFar::point
static bool point(yarp::dev::ICartesianControl *iarm, const yarp::sig::Vector &q, const yarp::sig::Vector &x)
Definition: pointing_far.cpp:351
iCub::action::ActionPrimitivesWayPoint
Struct for defining way points used for movements in the operational space.
Definition: actionPrimitives.h:133
iCub::action::ActionPrimitivesWayPoint::o
yarp::sig::Vector o
The 4x1 Vector specifying the orientation of the waypoint in axis-angle representation.
Definition: actionPrimitives.h:143
iCub::action::ActionPrimitivesWayPoint::x
yarp::sig::Vector x
The 3x1 Vector specifyng the position of the waypoint [m].
Definition: actionPrimitives.h:137
iCub::action::ActionPrimitivesWayPoint::oEnabled
bool oEnabled
If this flag is set to true then orientation will be taken into account.
Definition: actionPrimitives.h:149
iCub::action::ActionPrimitivesWayPoint::duration
double duration
The time duration [s] to achieve the waypoint.
Definition: actionPrimitives.h:156
Generated on Thu Apr 11 2024 13:51:00 for iCub-main by doxygen 1.9.1