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