iCub-main
solver.cpp
Go to the documentation of this file.
1 /*
2  * Copyright (C) 2010 RobotCub Consortium, European Commission FP6 Project IST-004370
3  * Author: Ugo Pattacini, Alessandro Roncone
4  * email: ugo.pattacini@iit.it, alessandro.roncone@iit.it
5  * website: www.robotcub.org
6  * Permission is granted to copy, distribute, and/or modify this program
7  * under the terms of the GNU General Public License, version 2 or any
8  * later version published by the Free Software Foundation.
9  *
10  * A copy of the license can be found at
11  * http://www.robotcub.org/icub/license/gpl.txt
12  *
13  * This program is distributed in the hope that it will be useful, but
14  * WITHOUT ANY WARRANTY; without even the implied warranty of
15  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General
16  * Public License for more details
17 */
18 
19 #include <cmath>
20 #include <algorithm>
21 
22 #include <yarp/math/SVD.h>
23 #include <iCub/solver.h>
24 
25 
26 /************************************************************************/
27 EyePinvRefGen::EyePinvRefGen(PolyDriver *_drvTorso, PolyDriver *_drvHead,
28  ExchangeData *_commData, Controller *_ctrl,
29  const Vector &_counterRotGain, const unsigned int _period) :
30  PeriodicThread((double)_period/1000.0), drvTorso(_drvTorso), drvHead(_drvHead),
31  commData(_commData), ctrl(_ctrl), period(_period),
32  Ts(_period/1000.0), counterRotGain(_counterRotGain)
33 {
34  // Instantiate objects
36  eyeL=new iCubEye("left_v"+commData->head_version.get_version());
37  eyeR=new iCubEye("right_v"+commData->head_version.get_version());
39 
40  // remove constraints on the links: logging purpose
41  imu->setAllConstraints(false);
42 
43  // block neck dofs
44  eyeL->blockLink(3,0.0); eyeR->blockLink(3,0.0);
45  eyeL->blockLink(4,0.0); eyeR->blockLink(4,0.0);
46  eyeL->blockLink(5,0.0); eyeR->blockLink(5,0.0);
47 
48  // Get the chain objects
52 
53  // add aligning matrices read from configuration file
54  getAlignHN(commData->rf_cameras,"ALIGN_KIN_LEFT",eyeL->asChain());
55  getAlignHN(commData->rf_cameras,"ALIGN_KIN_RIGHT",eyeR->asChain());
56 
57  // overwrite aligning matrices iff specified through tweak values
59  {
60  getAlignHN(commData->rf_tweak,"ALIGN_KIN_LEFT",eyeL->asChain());
61  getAlignHN(commData->rf_tweak,"ALIGN_KIN_RIGHT",eyeR->asChain());
62  }
63 
65  {
66  getAlignHN(commData->rf_tweak,"ALIGN_KIN_LEFT",eyeL->asChain());
67  getAlignHN(commData->rf_tweak,"ALIGN_KIN_RIGHT",eyeR->asChain());
68  }
69 
70  // get the length of the half of the eyes baseline
71  eyesHalfBaseline=0.5*norm(eyeL->EndEffPose().subVector(0,2)-eyeR->EndEffPose().subVector(0,2));
72 
73  // read number of joints
74  if (drvTorso!=nullptr)
75  {
76  IEncoders *encTorso; drvTorso->view(encTorso);
77  encTorso->getAxes(&nJointsTorso);
78  }
79  else
80  nJointsTorso=3;
81 
82  IEncoders *encHead; drvHead->view(encHead);
83  encHead->getAxes(&nJointsHead);
84 
85  // joints bounds alignment
89 
90  // just eye part is required
91  lim=lim.submatrix(3,5,0,1);
92 
93  // reinforce vergence min bound
95 
96  // read starting position
97  fbTorso.resize(nJointsTorso,0.0);
98  fbHead.resize(nJointsHead,0.0);
100 
101  // save original eyes tilt and pan bounds
102  orig_eye_tilt_min=(*chainEyeL)[nJointsTorso+3].getMin();
103  orig_eye_tilt_max=(*chainEyeL)[nJointsTorso+3].getMax();
104  orig_eye_pan_min=(*chainEyeL)[nJointsTorso+4].getMin();
105  orig_eye_pan_max=(*chainEyeL)[nJointsTorso+4].getMax();
106  orig_lim=lim;
107 
108  // Instantiate integrator
109  qd.resize(3);
110  qd[0]=fbHead[3];
111  qd[1]=fbHead[4];
112  qd[2]=fbHead[5];
113  I=new Integrator(Ts,qd,lim);
114 
115  fp.resize(3,0.0);
116  eyesJ.resize(3,3);
117  eyesJ.zero();
118 
119  genOn=false;
120  saccadeUnderWayOld=false;
121 }
122 
123 
124 /************************************************************************/
126 {
127  delete neck;
128  delete eyeL;
129  delete eyeR;
130  delete imu;
131  delete I;
132 }
133 
134 
135 /************************************************************************/
137 {
138  lock_guard<mutex> lck(mtx);
140  I->setLim(lim);
141  orig_lim=lim;
142 }
143 
144 
145 /************************************************************************/
146 bool EyePinvRefGen::bindEyes(const double ver)
147 {
148  lock_guard<mutex> lck(mtx);
149  if (ver>=0.0)
150  {
151  double ver_rad=std::max(CTRL_DEG2RAD*ver,commData->minAllowedVergence);
153 
154  // block pan of the left eye
155  (*chainEyeL)[nJointsTorso+4].setMin(ver_rad/2.0);
156  (*chainEyeL)[nJointsTorso+4].setMax(ver_rad/2.0);
157 
158  // block pan of the right eye
159  (*chainEyeR)[nJointsTorso+4].setMin(-ver_rad/2.0);
160  (*chainEyeR)[nJointsTorso+4].setMax(-ver_rad/2.0);
161 
162  // change the limits of the integrator
163  lim(1,0)=lim(1,1)=0.0;
164  lim(2,0)=lim(2,1)=ver_rad;
165  I->setLim(lim);
166 
167  qd[0]=fbHead[3];
168  qd[1]=0.0;
169  qd[2]=ver_rad;
170  I->reset(qd);
171 
172  yInfo("eyes constrained at vergence %g deg",commData->eyesBoundVer);
173  ctrl->look(fp);
174  return true;
175  }
176  else
177  return false;
178 }
179 
180 
181 /************************************************************************/
183 {
184  lock_guard<mutex> lck(mtx);
185  if (commData->eyesBoundVer>=0.0)
186  {
187  commData->eyesBoundVer=-1.0;
188 
189  // reinstate pan bound of the left eye
190  (*chainEyeL)[nJointsTorso+4].setMin(orig_eye_pan_min);
191  (*chainEyeL)[nJointsTorso+4].setMax(orig_eye_pan_max);
192 
193  // reinstate pan bound of the right eye
194  (*chainEyeR)[nJointsTorso+4].setMin(orig_eye_pan_min);
195  (*chainEyeR)[nJointsTorso+4].setMax(orig_eye_pan_max);
196 
197  // reinstate the limits of the integrator
198  lim=orig_lim;
199  I->setLim(lim);
200 
201  yInfo("eyes cleared");
202  return true;
203  }
204  else
205  return false;
206 }
207 
208 
209 /************************************************************************/
210 void EyePinvRefGen::manageBindEyes(const double ver)
211 {
212  if (!bindEyes(ver))
213  clearEyes();
214 }
215 
216 
217 /************************************************************************/
219 {
220  lock_guard<mutex> lck(mtx);
221  return counterRotGain;
222 }
223 
224 
225 /************************************************************************/
226 void EyePinvRefGen::setCounterRotGain(const Vector &gain)
227 {
228  lock_guard<mutex> lck(mtx);
229  size_t len=std::min(counterRotGain.length(),gain.length());
230  for (size_t i=0; i<len; i++)
231  counterRotGain[i]=gain[i];
232  yInfo("counter-rotation gains set to (%s)",counterRotGain.toString(3,3).c_str());
233 }
234 
235 
236 /************************************************************************/
237 Vector EyePinvRefGen::getEyesCounterVelocity(const Matrix &eyesJ, const Vector &fp)
238 {
239  // ********** implement VOR
240  Matrix H=imu->getH(cat(fbTorso,fbHead.subVector(0,2)));
241  H(0,3)=fp[0]-H(0,3);
242  H(1,3)=fp[1]-H(1,3);
243  H(2,3)=fp[2]-H(2,3);
244 
245  // gyro rate [rad/s]
246  Vector gyro{commData->get_gyro().first};
247 
248  // filter out the noise on the gyro readouts
250  gyro=0.0;
251 
252  Vector vor_fprelv=(gyro[0]*cross(H,0,H,3)+gyro[1]*cross(H,1,H,3)+gyro[2]*cross(H,2,H,3));
253 
254  // ********** implement OCR
255  H=chainNeck->getH();
256  Vector fph=fp;
257  fph.push_back(1.0);
258  fph=SE3inv(H)*fph;
259  Matrix HN=eye(4,4);
260  HN.setSubcol(fph,0,3);
261 
262  chainNeck->setHN(HN);
263  Vector ocr_fprelv=chainNeck->GeoJacobian()*commData->get_v().subVector(0,2);
264  ocr_fprelv=ocr_fprelv.subVector(0,2);
265  chainNeck->setHN(eye(4,4));
266 
267  // ********** blend the contributions
268  return -1.0*(pinv(eyesJ)*(counterRotGain[0]*vor_fprelv+counterRotGain[1]*ocr_fprelv));
269 }
270 
271 
272 /************************************************************************/
274 {
275  yInfo("Starting Pseudoinverse Reference Generator at %d ms",period);
276 
278  saccadesClock=Time::now();
279 
280  return true;
281 }
282 
283 
284 /************************************************************************/
286 {
287 }
288 
289 
290 /************************************************************************/
292 {
293  if (s)
294  yInfo("Pseudoinverse Reference Generator started successfully");
295  else
296  yError("Pseudoinverse Reference Generator did not start!");
297 }
298 
299 
300 /************************************************************************/
302 {
303  if (genOn)
304  {
305  lock_guard<mutex> lck(mtx);
306  double timeStamp;
307 
308  // read encoders
313 
314  // get current target
315  Vector xd=commData->port_xd->get_xd();
316 
317  // update neck chain
321 
322  // ask for saccades (if possible)
325  {
326  Vector fph=xd; fph.push_back(1.0);
327  fph=SE3inv(chainNeck->getH())*fph; fph[3]=0.0;
328  double rot=CTRL_RAD2DEG*acos(fph[2]/norm(fph)); fph[3]=1.0;
329 
330  // estimate geometrically the target tilt and pan of the eyes
331  Vector ang(3,0.0);
332  ang[0]=-atan2(fph[1],fabs(fph[2]));
333  ang[1]=atan2(fph[0],fph[2]);
334 
335  // enforce joints bounds
336  ang[0]=sat(ang[0],lim(0,0),lim(0,1));
337  ang[1]=sat(ang[1],lim(1,0),lim(1,1));
338 
339  // favor the smooth-pursuit in case saccades are small
341  {
342  // init vergence
343  ang[2]=fbHead[5];
344 
345  // get rid of eyes tilt
346  Vector axis(4);
347  axis[0]=1.0; axis[1]=0.0; axis[2]=0.0; axis[3]=-ang[0];
348  fph=axis2dcm(axis)*fph;
349 
350  // go on iff the point is in front of us
351  if (fph[2]>0.0)
352  {
353  double L,R;
354 
355  // estimate geometrically the target vergence Vg=L-R
356  if (fph[0]>=eyesHalfBaseline)
357  {
358  L=M_PI/2.0-atan2(fph[2],fph[0]+eyesHalfBaseline);
359  R=M_PI/2.0-atan2(fph[2],fph[0]-eyesHalfBaseline);
360  }
361  else if (fph[0]>-eyesHalfBaseline)
362  {
363  L=M_PI/2.0-atan2(fph[2],fph[0]+eyesHalfBaseline);
364  R=-(M_PI/2.0-atan2(fph[2],eyesHalfBaseline-fph[0]));
365  }
366  else
367  {
368  L=-(M_PI/2.0-atan2(fph[2],-fph[0]-eyesHalfBaseline));
369  R=-(M_PI/2.0-atan2(fph[2],eyesHalfBaseline-fph[0]));
370  }
371 
372  ang[2]=L-R;
373  }
374 
375  // enforce joints bounds
376  ang[2]=sat(ang[2],lim(2,0),lim(2,1));
377 
378  commData->set_qd(3,ang[0]);
379  commData->set_qd(4,ang[1]);
380  commData->set_qd(5,ang[2]);
381 
382  Vector vel(3,SACCADES_VEL);
383  ctrl->doSaccade(ang,vel);
384  saccadesClock=Time::now();
385  }
386  }
387 
388  // update eyes chains for convergence purpose
391  chainEyeL->setAng(nJointsTorso+4,qd[1]+qd[2]/2.0); chainEyeR->setAng(nJointsTorso+4,qd[1]-qd[2]/2.0);
392 
393  // converge on target
394  if (CartesianHelper::computeFixationPointData(*chainEyeL,*chainEyeR,fp,eyesJ))
395  {
396  Vector v=EYEPINVREFGEN_GAIN*(pinv(eyesJ)*(xd-fp));
397 
398  // update eyes chains in actual configuration for velocity compensation
401 
402  // compensate neck rotation at eyes level
403  if ((commData->eyesBoundVer>=0.0) || !CartesianHelper::computeFixationPointData(*chainEyeL,*chainEyeR,fp,eyesJ))
404  commData->set_counterv(zeros((int)qd.length()));
405  else
407 
408  // reset eyes controller and integral upon saccades transition on=>off
410  {
411  ctrl->resetCtrlEyes();
412 
413  qd[0]=fbHead[3];
414  qd[1]=fbHead[4];
415  qd[2]=fbHead[5];
416  I->reset(qd);
417  }
418 
419  // update reference
421  }
422  else
423  commData->set_counterv(zeros((int)qd.length()));
424 
425  // set a new target position
426  commData->set_xd(xd);
427  commData->set_x(fp,timeStamp);
430  {
431  commData->set_qd(3,qd[0]);
432  commData->set_qd(4,qd[1]);
433  commData->set_qd(5,qd[2]);
434  }
435 
436  // latch the saccades status
439  }
440 }
441 
442 
443 /************************************************************************/
445 {
446  PeriodicThread::suspend();
447  yInfo("Pseudoinverse Reference Generator has been suspended!");
448 }
449 
450 
451 /************************************************************************/
453 {
454  PeriodicThread::resume();
455  yInfo("Pseudoinverse Reference Generator has been resumed!");
456 }
457 
458 
459 /************************************************************************/
460 Solver::Solver(PolyDriver *_drvTorso, PolyDriver *_drvHead, ExchangeData *_commData,
461  EyePinvRefGen *_eyesRefGen, Localizer *_loc, Controller *_ctrl,
462  const unsigned int _period) :
463  PeriodicThread((double)_period/1000.0), drvTorso(_drvTorso), drvHead(_drvHead),
464  commData(_commData), eyesRefGen(_eyesRefGen), loc(_loc),
465  ctrl(_ctrl), period(_period), Ts(_period/1000.0)
466 {
467  // Instantiate objects
469  eyeL=new iCubEye("left_v"+commData->head_version.get_version());
470  eyeR=new iCubEye("right_v"+commData->head_version.get_version());
472  torsoVel=new AWLinEstimator(16,0.5);
473 
474  // remove constraints on the links: logging purpose
475  imu->setAllConstraints(false);
476 
477  // block neck dofs
478  eyeL->blockLink(3,0.0); eyeR->blockLink(3,0.0);
479  eyeL->blockLink(4,0.0); eyeR->blockLink(4,0.0);
480  eyeL->blockLink(5,0.0); eyeR->blockLink(5,0.0);
481 
482  // Get the chain objects
484  chainEyeL=eyeL->asChain();
486 
487  invNeck=new GazeIpOptMin(*chainNeck,1e-3,1e-3,20);
488 
489  // add aligning matrices read from configuration file
490  getAlignHN(commData->rf_cameras,"ALIGN_KIN_LEFT",eyeL->asChain());
491  getAlignHN(commData->rf_cameras,"ALIGN_KIN_RIGHT",eyeR->asChain());
492 
493  // overwrite aligning matrices iff specified through tweak values
495  {
496  getAlignHN(commData->rf_tweak,"ALIGN_KIN_LEFT",eyeL->asChain());
497  getAlignHN(commData->rf_tweak,"ALIGN_KIN_RIGHT",eyeR->asChain());
498  }
499 
500  // read number of joints
501  if (drvTorso!=nullptr)
502  {
503  IEncoders *encTorso; drvTorso->view(encTorso);
504  encTorso->getAxes(&nJointsTorso);
505  }
506  else
507  nJointsTorso=3;
508 
509  IEncoders *encHead; drvHead->view(encHead);
510  encHead->getAxes(&nJointsHead);
511 
512  // joints bounds alignment
514 
515  // read starting position
516  fbTorso.resize(nJointsTorso,0.0);
517  fbHead.resize(nJointsHead,0.0);
519 
522 
523  // store neck pitch/roll/yaw bounds
524  neckPitchMin=(*chainNeck)[3].getMin();
525  neckPitchMax=(*chainNeck)[3].getMax();
526  neckRollMin =(*chainNeck)[4].getMin();
527  neckRollMax =(*chainNeck)[4].getMax();
528  neckYawMin =(*chainNeck)[5].getMin();
529  neckYawMax =(*chainNeck)[5].getMax();
530 
531  neckPos.resize(3);
532  gazePos.resize(3);
533  updateAngles();
534 
536  chainNeck->setAng(3,fbHead[0]);
537  chainNeck->setAng(4,fbHead[1]);
538  chainNeck->setAng(5,fbHead[2]);
539 
544 
545  Vector eyePos(2);
546  eyePos[0]=gazePos[0];
547  eyePos[1]=gazePos[1]+gazePos[2]/2.0;
548  chainEyeL->setAng(eyePos);
549  eyePos[1]=gazePos[1]-gazePos[2]/2.0;
550  chainEyeR->setAng(eyePos);
551 
553 }
554 
555 
556 /************************************************************************/
558 {
559  delete neck;
560  delete eyeL;
561  delete eyeR;
562  delete imu;
563  delete torsoVel;
564  delete invNeck;
565 }
566 
567 
568 /************************************************************************/
569 void Solver::bindNeckPitch(const double min_deg, const double max_deg)
570 {
571  lock_guard<mutex> lck(mtx);
572  double min_rad=sat(CTRL_DEG2RAD*min_deg,neckPitchMin,neckPitchMax);
573  double max_rad=sat(CTRL_DEG2RAD*max_deg,neckPitchMin,neckPitchMax);
574 
575  (*chainNeck)(0).setMin(min_rad);
576  (*chainNeck)(0).setMax(max_rad);
577 
578  yInfo("neck pitch constrained in [%g,%g] deg",min_deg,max_deg);
579 }
580 
581 
582 /************************************************************************/
583 void Solver::bindNeckRoll(const double min_deg, const double max_deg)
584 {
585  lock_guard<mutex> lck(mtx);
586  double min_rad=sat(CTRL_DEG2RAD*min_deg,neckRollMin,neckRollMax);
587  double max_rad=sat(CTRL_DEG2RAD*max_deg,neckRollMin,neckRollMax);
588 
589  (*chainNeck)(1).setMin(min_rad);
590  (*chainNeck)(1).setMax(max_rad);
591 
592  yInfo("neck roll constrained in [%g,%g] deg",min_deg,max_deg);
593 }
594 
595 
596 /************************************************************************/
597 void Solver::bindNeckYaw(const double min_deg, const double max_deg)
598 {
599  lock_guard<mutex> lck(mtx);
600  double min_rad=sat(CTRL_DEG2RAD*min_deg,neckYawMin,neckYawMax);
601  double max_rad=sat(CTRL_DEG2RAD*max_deg,neckYawMin,neckYawMax);
602 
603  (*chainNeck)(2).setMin(min_rad);
604  (*chainNeck)(2).setMax(max_rad);
605 
606  yInfo("neck yaw constrained in [%g,%g] deg",min_deg,max_deg);
607 }
608 
609 
610 /************************************************************************/
611 void Solver::getCurNeckPitchRange(double &min_deg, double &max_deg)
612 {
613  lock_guard<mutex> lck(mtx);
614  min_deg=CTRL_RAD2DEG*(*chainNeck)(0).getMin();
615  max_deg=CTRL_RAD2DEG*(*chainNeck)(0).getMax();
616 }
617 
618 
619 /************************************************************************/
620 void Solver::getCurNeckRollRange(double &min_deg, double &max_deg)
621 {
622  lock_guard<mutex> lck(mtx);
623  min_deg=CTRL_RAD2DEG*(*chainNeck)(1).getMin();
624  max_deg=CTRL_RAD2DEG*(*chainNeck)(1).getMax();
625 }
626 
627 
628 /************************************************************************/
629 void Solver::getCurNeckYawRange(double &min_deg, double &max_deg)
630 {
631  lock_guard<mutex> lck(mtx);
632  min_deg=CTRL_RAD2DEG*(*chainNeck)(2).getMin();
633  max_deg=CTRL_RAD2DEG*(*chainNeck)(2).getMax();
634 }
635 
636 
637 /************************************************************************/
639 {
640  lock_guard<mutex> lck(mtx);
641  (*chainNeck)(0).setMin(neckPitchMin);
642  (*chainNeck)(0).setMax(neckPitchMax);
643 
644  yInfo("neck pitch cleared");
645 }
646 
647 
648 /************************************************************************/
650 {
651  lock_guard<mutex> lck(mtx);
652  (*chainNeck)(1).setMin(neckRollMin);
653  (*chainNeck)(1).setMax(neckRollMax);
654 
655  yInfo("neck roll cleared");
656 }
657 
658 
659 /************************************************************************/
661 {
662  lock_guard<mutex> lck(mtx);
663  (*chainNeck)(2).setMin(neckYawMin);
664  (*chainNeck)(2).setMax(neckYawMax);
665 
666  yInfo("neck yaw cleared");
667 }
668 
669 
670 /************************************************************************/
672 {
673  return neckAngleUserTolerance;
674 }
675 
676 
677 /************************************************************************/
678 void Solver::setNeckAngleUserTolerance(const double angle)
679 {
680  neckAngleUserTolerance=fabs(angle);
681 }
682 
683 
684 /************************************************************************/
686 {
687  neckPos[0]=fbHead[0];
688  neckPos[1]=fbHead[1];
689  neckPos[2]=fbHead[2];
690  gazePos[0]=fbHead[3];
691  gazePos[1]=fbHead[4];
692  gazePos[2]=fbHead[5];
693 }
694 
695 
696 /************************************************************************/
697 double Solver::neckTargetRotAngle(const Vector &xd)
698 {
699  Matrix H=commData->get_fpFrame();
700  Vector xdh=xd; xdh.push_back(1.0);
701  xdh=SE3inv(H)*xdh; xdh[3]=0.0;
702 
703  return (CTRL_RAD2DEG*acos(xdh[2]/norm(xdh)));
704 }
705 
706 
707 /************************************************************************/
708 Vector Solver::computeTargetUserTolerance(const Vector &xd)
709 {
710  Matrix H=commData->get_fpFrame();
711  Vector z(3,0.0); z[2]=1.0;
712  Vector xdh=xd; xdh.push_back(1.0);
713  xdh=SE3inv(H)*xdh;
714 
715  Vector xdh3=xdh; xdh3.pop_back();
716  Vector rot=cross(xdh3,z);
717  double r=norm(rot);
718  if (r<IKIN_ALMOST_ZERO)
719  return xd;
720 
721  rot=rot/r;
722  rot.push_back(neckAngleUserTolerance*CTRL_DEG2RAD);
723  rot=H*(axis2dcm(rot)*xdh);
724  rot.pop_back();
725 
726  return rot;
727 }
728 
729 
730 /************************************************************************/
732 {
733  // Initialization
734  Vector fp;
735  CartesianHelper::computeFixationPointData(*chainEyeL,*chainEyeR,fp);
736 
737  // init commData structure
738  commData->port_xd->init(fp);
739  commData->set_xd(fp);
741  commData->set_x(fp);
744  commData->resize_v((int)fbHead.length(),0.0);
745  commData->resize_counterv(3,0.0);
747 
748  // use eyes pseudoinverse reference generator
749  eyesRefGen->enable();
750 
751  yInfo("Starting Solver at %d ms",period);
752  return true;
753 }
754 
755 
756 /************************************************************************/
758 {
759  eyesRefGen->disable();
760 }
761 
762 
763 /************************************************************************/
764 void Solver::afterStart(bool s)
765 {
766  if (s)
767  yInfo("Solver started successfully");
768  else
769  yError("Solver did not start!");
770 }
771 
772 
773 /************************************************************************/
775 {
776  typedef enum { ctrl_off, ctrl_wait, ctrl_on } cstate;
777  static cstate state_=ctrl_off;
778 
779  lock_guard<mutex> lck(mtx);
780 
781  // get the current target
782  Vector xd=commData->port_xd->get_xdDelayed();
783 
784  // update the target straightaway
785  commData->set_xd(xd);
786 
787  // read encoders
792 
793  torsoVel->feedData(AWPolyElement(fbTorso,Time::now()));
795 
796  // update kinematics
797  updateAngles();
801 
802  // hereafter accumulate solving conditions: the order does matter
803 
804  // 1) compute the angular distance
805  double theta=neckTargetRotAngle(xd);
806  bool doSolve=(theta>NECKSOLVER_ACTIVATIONANGLE);
807 
808  // 2) skip if controller is active and no torso motion is detected
809  doSolve&=!(commData->ctrlActive && !torsoChanged);
810 
811  // 3) skip if controller is inactive and we are not in tracking mode
813 
814  // 4) solve straightaway if we are in tracking mode and torso motion is detected
815  doSolve|=commData->trackingModeOn && torsoChanged;
816 
817  // 5) solve straightaway if the target has changed
818  doSolve|=commData->port_xd->get_newDelayed();
819 
820  // 6) skip if the angle to target is lower than the user tolerance
821  doSolve&=(theta>neckAngleUserTolerance);
822 
823  // clear triggers
824  commData->port_xd->get_newDelayed()=false;
825 
826  // call the solver for neck
827  if (doSolve)
828  {
829  Vector gDir(3,0.0); gDir[2]=-1.0;
831  {
832  Vector acc=-1.0*commData->get_accel().first;
833  Matrix H=imu->getH(cat(fbTorso,fbHead.subVector(0,2)));
834  gDir=H.submatrix(0,2,0,2)*acc;
835  }
836 
837  Vector xdUserTol=computeTargetUserTolerance(xd);
838  neckPos=invNeck->solve(neckPos,xdUserTol,gDir);
839 
840  // update neck pitch,roll,yaw
841  commData->set_qd(0,neckPos[0]);
842  commData->set_qd(1,neckPos[1]);
843  commData->set_qd(2,neckPos[2]);
845 
846  state_=ctrl_wait;
847  }
848 
849  if (state_==ctrl_off)
850  {
851  // keep neck targets equal to current angles
852  // to avoid glitches in the control,
853  // especially during stabilization
854  commData->set_qd(0,neckPos[0]);
855  commData->set_qd(1,neckPos[1]);
856  commData->set_qd(2,neckPos[2]);
857  }
858  else if (state_==ctrl_wait)
859  {
860  if (commData->ctrlActive)
861  state_=ctrl_on;
862  }
863  else if (state_==ctrl_on)
864  {
865  if (!commData->ctrlActive)
866  state_=ctrl_off;
867  }
868 }
869 
870 
871 /************************************************************************/
873 {
874  commData->port_xd->lock();
875  PeriodicThread::suspend();
876  yInfo("Solver has been suspended!");
877 }
878 
879 
880 /************************************************************************/
882 {
883  lock_guard<mutex> lck(mtx);
884 
885  // read encoders
890 
891  // update kinematics
892  updateAngles();
898 
899  torsoVel->reset();
900  commData->port_xd->unlock();
901 
902  PeriodicThread::resume();
903  yInfo("Solver has been resumed!");
904 }
905 
906 
907 
#define CTRL_RAD2DEG
Definition: XSensMTx.cpp:25
#define CTRL_DEG2RAD
Definition: XSensMTx.cpp:26
#define M_PI
Definition: XSensMTx.cpp:24
bool look(const Vector &x)
Definition: controller.cpp:519
void doSaccade(const Vector &ang, const Vector &vel)
Definition: controller.cpp:476
void resetCtrlEyes()
Definition: controller.cpp:538
ResourceFinder rf_cameras
Definition: utils.h:156
double saccadesActivationAngle
Definition: utils.h:145
Vector get_v()
Definition: utils.cpp:319
bool stabilizationOn
Definition: utils.h:154
bool trackingModeOn
Definition: utils.h:148
std::pair< Vector, bool > get_gyro()
Definition: utils.cpp:345
void set_fpFrame(const Matrix &_S)
Definition: utils.cpp:257
void set_xd(const Vector &_xd)
Definition: utils.cpp:184
double gyro_noise_threshold
Definition: utils.h:141
void set_qd(const Vector &_qd)
Definition: utils.cpp:192
void set_q(const Vector &_q)
Definition: utils.cpp:225
double saccadesInhibitionPeriod
Definition: utils.h:144
bool tweakOverwrite
Definition: utils.h:151
std::pair< Vector, bool > get_accel()
Definition: utils.cpp:362
Vector get_counterv()
Definition: utils.cpp:328
ResourceFinder rf_tweak
Definition: utils.h:157
void resize_counterv(const int sz, const double val)
Definition: utils.cpp:176
bool ctrlActive
Definition: utils.h:147
Matrix get_fpFrame()
Definition: utils.cpp:337
double minAllowedVergence
Definition: utils.h:139
int neckSolveCnt
Definition: utils.h:146
xdPort * port_xd
Definition: utils.h:135
void resize_v(const int sz, const double val)
Definition: utils.cpp:168
bool saccadesOn
Definition: utils.h:152
iKinLimbVersion head_version
Definition: utils.h:143
void set_x(const Vector &_x)
Definition: utils.cpp:208
bool saccadeUnderway
Definition: utils.h:149
void set_counterv(const Vector &_counterv)
Definition: utils.cpp:249
double eyesBoundVer
Definition: utils.h:140
void set_torso(const Vector &_torso)
Definition: utils.cpp:233
iCubInertialSensor * imu
Definition: solver.h:62
double eyesHalfBaseline
Definition: solver.h:82
Vector counterRotGain
Definition: solver.h:90
mutex mtx
Definition: solver.h:68
double orig_eye_pan_max
Definition: solver.h:73
Integrator * I
Definition: solver.h:67
iKinChain * chainNeck
Definition: solver.h:63
void run() override
Definition: solver.cpp:301
void minAllowedVergenceChanged() override
Definition: solver.cpp:136
bool saccadeUnderWayOld
Definition: solver.h:76
EyePinvRefGen(PolyDriver *_drvTorso, PolyDriver *_drvHead, ExchangeData *_commData, Controller *_ctrl, const Vector &_counterRotGain, const unsigned int _period)
Definition: solver.cpp:27
bool clearEyes()
Definition: solver.cpp:182
Vector getCounterRotGain()
Definition: solver.cpp:218
void afterStart(bool s) override
Definition: solver.cpp:291
PolyDriver * drvTorso
Definition: solver.h:64
void manageBindEyes(const double ver)
Definition: solver.cpp:210
void enable()
Definition: solver.h:99
Matrix lim
Definition: solver.h:85
bool bindEyes(const double ver)
Definition: solver.cpp:146
ExchangeData * commData
Definition: solver.h:65
int nJointsTorso
Definition: solver.h:78
Vector fbTorso
Definition: solver.h:86
void threadRelease() override
Definition: solver.cpp:285
void suspend()
Definition: solver.cpp:444
Vector fbHead
Definition: solver.h:87
double orig_eye_tilt_max
Definition: solver.h:71
Controller * ctrl
Definition: solver.h:66
iKinChain * chainEyeR
Definition: solver.h:63
bool threadInit() override
Definition: solver.cpp:273
double orig_eye_tilt_min
Definition: solver.h:70
void disable()
Definition: solver.h:100
Vector fp
Definition: solver.h:88
Vector getEyesCounterVelocity(const Matrix &eyesJ, const Vector &fp)
Definition: solver.cpp:237
iKinChain * chainEyeL
Definition: solver.h:63
iCubHeadCenter * neck
Definition: solver.h:61
virtual ~EyePinvRefGen()
Definition: solver.cpp:125
double saccadesClock
Definition: solver.h:81
bool genOn
Definition: solver.h:77
void setCounterRotGain(const Vector &gain)
Definition: solver.cpp:226
PolyDriver * drvHead
Definition: solver.h:64
Vector qd
Definition: solver.h:88
void resume()
Definition: solver.cpp:452
Matrix orig_lim
Definition: solver.h:85
int nJointsHead
Definition: solver.h:79
double orig_eye_pan_min
Definition: solver.h:72
double Ts
Definition: solver.h:83
unsigned int period
Definition: solver.h:75
Matrix eyesJ
Definition: solver.h:89
int saccadesRxTargets
Definition: solver.h:80
iCubEye * eyeR
Definition: utils.h:173
iCubEye * eyeL
Definition: utils.h:172
Vector solve(const Vector &q0, Vector &xd, const Vector &gDir)
Definition: gazeNlp.cpp:332
Solver(PolyDriver *_drvTorso, PolyDriver *_drvHead, ExchangeData *_commData, EyePinvRefGen *_eyesRefGen, Localizer *_loc, Controller *_ctrl, const unsigned int _period)
Definition: solver.cpp:460
unsigned int period
Definition: solver.h:133
void threadRelease() override
Definition: solver.cpp:757
void run() override
Definition: solver.cpp:774
PolyDriver * drvHead
Definition: solver.h:126
void bindNeckPitch(const double min_deg, const double max_deg)
Definition: solver.cpp:569
void getCurNeckYawRange(double &min_deg, double &max_deg)
Definition: solver.cpp:629
void clearNeckYaw()
Definition: solver.cpp:660
double neckRollMin
Definition: solver.h:148
int nJointsHead
Definition: solver.h:135
bool threadInit() override
Definition: solver.cpp:731
iCubHeadCenter * neck
Definition: solver.h:122
ExchangeData * commData
Definition: solver.h:127
void getCurNeckPitchRange(double &min_deg, double &max_deg)
Definition: solver.cpp:611
mutex mtx
Definition: solver.h:131
double neckPitchMin
Definition: solver.h:146
EyePinvRefGen * eyesRefGen
Definition: solver.h:128
void suspend()
Definition: solver.cpp:872
iKinChain * chainNeck
Definition: solver.h:124
double neckYawMin
Definition: solver.h:150
void clearNeckRoll()
Definition: solver.cpp:649
Vector fbHead
Definition: solver.h:140
Vector computeTargetUserTolerance(const Vector &xd)
Definition: solver.cpp:708
Vector neckPos
Definition: solver.h:141
iKinChain * chainEyeR
Definition: solver.h:124
double getNeckAngleUserTolerance() const
Definition: solver.cpp:671
void updateAngles()
Definition: solver.cpp:685
double neckTargetRotAngle(const Vector &xd)
Definition: solver.cpp:697
iKinChain * chainEyeL
Definition: solver.h:124
void bindNeckYaw(const double min_deg, const double max_deg)
Definition: solver.cpp:597
void clearNeckPitch()
Definition: solver.cpp:638
iCubInertialSensor * imu
Definition: solver.h:123
double neckAngleUserTolerance
Definition: solver.h:136
void setNeckAngleUserTolerance(const double angle)
Definition: solver.cpp:678
int nJointsTorso
Definition: solver.h:134
PolyDriver * drvTorso
Definition: solver.h:126
virtual ~Solver()
Definition: solver.cpp:557
void resume()
Definition: solver.cpp:881
void getCurNeckRollRange(double &min_deg, double &max_deg)
Definition: solver.cpp:620
GazeIpOptMin * invNeck
Definition: solver.h:125
Vector fbTorso
Definition: solver.h:139
double neckYawMax
Definition: solver.h:151
void afterStart(bool s) override
Definition: solver.cpp:764
double neckRollMax
Definition: solver.h:149
double neckPitchMax
Definition: solver.h:147
Vector gazePos
Definition: solver.h:142
AWLinEstimator * torsoVel
Definition: solver.h:144
void bindNeckRoll(const double min_deg, const double max_deg)
Definition: solver.cpp:583
Adaptive window linear fitting to estimate the first derivative.
Basic element for adaptive polynomial fitting.
yarp::sig::Vector estimate()
Execute the algorithm upon the elements list, with the max deviation threshold given by D.
void feedData(const AWPolyElement &el)
Feed data into the algorithm.
void reset()
Reinitialize the internal state.
A class for defining a saturated integrator based on Tustin formula: .
Definition: pids.h:48
const yarp::sig::Vector & integrate(const yarp::sig::Vector &x)
Executes one-step integration of input vector.
Definition: pids.cpp:115
void reset(const yarp::sig::Vector &y0)
Resets the internal state and sets the output vector to the given value.
Definition: pids.cpp:128
void setLim(const yarp::sig::Matrix &_lim)
Sets the output vector constraints matrix.
Definition: pids.cpp:104
A class for defining the iCub Eye.
Definition: iKinFwd.h:1386
A class for describing the kinematic of the straight line coming out from the point located between t...
Definition: iKinFwd.h:1451
A class for defining the Inertia Sensor Kinematics of the iCub.
Definition: iKinFwd.h:1477
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
bool setHN(const yarp::sig::Matrix &_HN)
Sets HN, the rigid roto-translation matrix from the Nth frame to the end-effector.
Definition: iKinFwd.cpp:580
yarp::sig::Matrix GeoJacobian(const unsigned int i)
Returns the geometric Jacobian of the ith link.
Definition: iKinFwd.cpp:1012
yarp::sig::Vector EndEffPose(const bool axisRep=true)
Returns the coordinates of end-effector.
Definition: iKinFwd.cpp:850
yarp::sig::Vector setAng(const yarp::sig::Vector &q)
Sets the free joint angles to values of q[i].
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
void setAllConstraints(bool _constrained)
Sets the constraint status of all chain links.
Definition: iKinFwd.cpp:498
std::string get_version() const
Return the version string.
Definition: iKinFwd.cpp:1600
iKinChain * asChain()
Returns a pointer to the Limb seen as Chain object.
Definition: iKinFwd.h:1013
Vector get_xdDelayed()
Definition: utils.cpp:107
int get_rx() const
Definition: utils.h:83
void lock()
Definition: utils.h:80
void init(const Vector &xd0)
Definition: utils.cpp:49
Vector get_xd()
Definition: utils.cpp:98
bool & get_newDelayed()
Definition: utils.h:85
void unlock()
Definition: utils.h:81
zeros(2, 2) eye(2
_3f_vect_t acc
Definition: dataTypes.h:1
double norm(const yarp::sig::Matrix &M, int col)
Returns the norm of the vector given in the form: matrix(:,col).
yarp::sig::Vector cross(const yarp::sig::Matrix &A, int colA, const yarp::sig::Matrix &B, int colB)
Returns the cross product between two vectors given in the form: matrix(:,col).
#define IKIN_ALMOST_ZERO
Definition: iKinHlp.h:27
void copyJointsBounds(iKinChain *ch1, iKinChain *ch2)
Definition: utils.cpp:602
bool getAlignHN(const ResourceFinder &rf, const string &type, iKinChain *chain, const bool verbose=false)
Definition: utils.cpp:474
bool getFeedback(Vector &fbTorso, Vector &fbHead, PolyDriver *drvTorso, PolyDriver *drvHead, const ExchangeData *commData, double *timeStamp=nullptr)
Definition: utils.cpp:633
void updateNeckBlockedJoints(iKinChain *chain, const Vector &fbNeck)
Definition: utils.cpp:625
void updateTorsoBlockedJoints(iKinChain *chain, const Vector &fbTorso)
Definition: utils.cpp:617
Matrix alignJointsBounds(iKinChain *chain, PolyDriver *drvTorso, PolyDriver *drvHead, const ExchangeData *commData)
Definition: utils.cpp:531
double sat(const double val, const double min, const double max)
Definition: utils.h:183
const FSC max
Definition: strain.h:48
const FSC min
Definition: strain.h:49
static struct bpf_program fp
constexpr double NECKSOLVER_ACTIVATIONANGLE_JOINTS
Definition: solver.h:42
constexpr double SACCADES_VEL
Definition: solver.h:38
constexpr double EYEPINVREFGEN_GAIN
Definition: solver.h:37
constexpr double NECKSOLVER_ACTIVATIONANGLE
Definition: solver.h:43