iCub-main
utils.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 <limits>
21 #include <algorithm>
22 #include <sstream>
23 
24 #include <iCub/utils.h>
25 #include <iCub/solver.h>
26 
27 #define MUTEX_XD 0
28 #define MUTEX_QD 1
29 #define MUTEX_X 2
30 #define MUTEX_Q 3
31 #define MUTEX_TORSO 4
32 #define MUTEX_V 5
33 #define MUTEX_COUNTERV 6
34 #define MUTEX_FPFRAME 7
35 #define MUTEX_IMU 8
36 
37 
38 /************************************************************************/
39 xdPort::xdPort(void *_slv) : slv(_slv)
40 {
41  isNewDelayed=isNew=false;
42  locked=false;
43  closing=false;
44  rx=0;
45 
46  useCallback();
47  start();
48 }
49 
50 
51 /************************************************************************/
52 void xdPort::init(const Vector &xd0)
53 {
54  xdDelayed=xd=xd0;
55 }
56 
57 
58 /************************************************************************/
60 {
61  closing=true;
62  triggerNeck.signal();
63  stop();
64 }
65 
66 
67 /************************************************************************/
68 void xdPort::onRead(Bottle &b)
69 {
70  LockGuard lg(mutex_0);
71  if (locked)
72  return;
73 
74  size_t n=std::min(b.size(),xd.length());
75  for (size_t i=0; i<n; i++)
76  xd[i]=b.get(i).asDouble();
77 
78  isNew=true;
79  rx++;
80 
81  triggerNeck.signal();
82 }
83 
84 
85 /************************************************************************/
86 bool xdPort::set_xd(const Vector &_xd)
87 {
88  LockGuard lg(mutex_0);
89  if (locked)
90  return false;
91 
92  xd=_xd;
93  isNew=true;
94  rx++;
95  triggerNeck.signal();
96  return true;
97 }
98 
99 
100 /************************************************************************/
102 {
103  LockGuard lg(mutex_0);
104  Vector _xd=xd;
105  return _xd;
106 }
107 
108 
109 /************************************************************************/
111 {
112  LockGuard lg(mutex_1);
113  Vector _xdDelayed=xdDelayed;
114  return _xdDelayed;
115 }
116 
117 
118 /************************************************************************/
120 {
121  while (!isStopping() && !closing)
122  {
123  triggerNeck.reset();
124  triggerNeck.wait();
125 
126  double timeDelay=0.0;
127  double theta=static_cast<Solver*>(slv)->neckTargetRotAngle(xd);
128  if (theta<NECKSOLVER_RESTORINGANGLE)
129  timeDelay=NECKSOLVER_ACTIVATIONDELAY;
130 
131  Time::delay(timeDelay);
132 
133  LockGuard lg(mutex_1);
134  xdDelayed=xd;
135  isNewDelayed=true;
136  }
137 }
138 
139 
140 /************************************************************************/
142 {
143  imu.resize(12,0.0);
144  port_xd=NULL;
145 
146  ctrlActive=false;
147  trackingModeOn=false;
148  saccadeUnderway=false;
149  minAllowedVergence=0.0;
150  eyesBoundVer=-1.0;
151  neckSolveCnt=0;
152 
153  saccadesInhibitionPeriod=SACCADES_INHIBITION_PERIOD;
154  saccadesActivationAngle=SACCADES_ACTIVATION_ANGLE;
155 
156  eyeTiltLim.resize(2);
157  eyeTiltLim[0]=-std::numeric_limits<double>::max();
158  eyeTiltLim[1]=std::numeric_limits<double>::max();
159 
160  robotName="";
161  localStemName="";
162  head_version=1.0;
163  tweakOverwrite=true;
164  tweakFile="";
165 }
166 
167 
168 /************************************************************************/
169 void ExchangeData::resize_v(const int sz, const double val)
170 {
171  LockGuard lg(mutex[MUTEX_V]);
172  v.resize(sz,val);
173 }
174 
175 
176 /************************************************************************/
177 void ExchangeData::resize_counterv(const int sz, const double val)
178 {
179  LockGuard lg(mutex[MUTEX_COUNTERV]);
180  counterv.resize(sz,val);
181 }
182 
183 
184 /************************************************************************/
185 void ExchangeData::set_xd(const Vector &_xd)
186 {
187  LockGuard lg(mutex[MUTEX_XD]);
188  xd=_xd;
189 }
190 
191 
192 /************************************************************************/
193 void ExchangeData::set_qd(const Vector &_qd)
194 {
195  LockGuard lg(mutex[MUTEX_QD]);
196  qd=_qd;
197 }
198 
199 
200 /************************************************************************/
201 void ExchangeData::set_qd(const int i, const double val)
202 {
203  LockGuard lg(mutex[MUTEX_QD]);
204  qd[i]=val;
205 }
206 
207 
208 /************************************************************************/
209 void ExchangeData::set_x(const Vector &_x)
210 {
211  LockGuard lg(mutex[MUTEX_X]);
212  x=_x;
213 }
214 
215 
216 /************************************************************************/
217 void ExchangeData::set_x(const Vector &_x, const double stamp)
218 {
219  LockGuard lg(mutex[MUTEX_X]);
220  x=_x;
221  x_stamp=stamp;
222 }
223 
224 
225 /************************************************************************/
226 void ExchangeData::set_q(const Vector &_q)
227 {
228  LockGuard lg(mutex[MUTEX_Q]);
229  q=_q;
230 }
231 
232 
233 /************************************************************************/
234 void ExchangeData::set_torso(const Vector &_torso)
235 {
236  LockGuard lg(mutex[MUTEX_TORSO]);
237  torso=_torso;
238 }
239 
240 
241 /************************************************************************/
242 void ExchangeData::set_v(const Vector &_v)
243 {
244  LockGuard lg(mutex[MUTEX_V]);
245  v=_v;
246 }
247 
248 
249 /************************************************************************/
250 void ExchangeData::set_counterv(const Vector &_counterv)
251 {
252  LockGuard lg(mutex[MUTEX_COUNTERV]);
253  counterv=_counterv;
254 }
255 
256 
257 /************************************************************************/
258 void ExchangeData::set_fpFrame(const Matrix &_S)
259 {
260  LockGuard lg(mutex[MUTEX_FPFRAME]);
261  S=_S;
262 }
263 
264 
265 /************************************************************************/
266 void ExchangeData::set_imu(const Vector &_imu)
267 {
268  LockGuard lg(mutex[MUTEX_IMU]);
269  imu=_imu;
270 }
271 
272 
273 /************************************************************************/
275 {
276  LockGuard lg(mutex[MUTEX_XD]);
277  Vector _xd=xd;
278  return _xd;
279 }
280 
281 
282 /************************************************************************/
284 {
285  LockGuard lg(mutex[MUTEX_QD]);
286  Vector _qd=qd;
287  return _qd;
288 }
289 
290 
291 /************************************************************************/
293 {
294  LockGuard lg(mutex[MUTEX_X]);
295  Vector _x=x;
296  return _x;
297 }
298 
299 
300 /************************************************************************/
301 Vector ExchangeData::get_x(double &stamp)
302 {
303  LockGuard lg(mutex[MUTEX_X]);
304  Vector _x=x;
305  stamp=x_stamp;
306  return _x;
307 }
308 
309 
310 /************************************************************************/
312 {
313  LockGuard lg(mutex[MUTEX_Q]);
314  Vector _q=q;
315  return _q;
316 }
317 
318 
319 /************************************************************************/
321 {
322  LockGuard lg(mutex[MUTEX_TORSO]);
323  Vector _torso=torso;
324  return _torso;
325 }
326 
327 
328 /************************************************************************/
330 {
331  LockGuard lg(mutex[MUTEX_V]);
332  Vector _v=v;
333  return _v;
334 }
335 
336 
337 /************************************************************************/
339 {
340  LockGuard lg(mutex[MUTEX_COUNTERV]);
341  Vector _counterv=counterv;
342  return _counterv;
343 }
344 
345 
346 /************************************************************************/
348 {
349  LockGuard lg(mutex[MUTEX_FPFRAME]);
350  Matrix _S=S;
351  return _S;
352 }
353 
354 
355 /************************************************************************/
357 {
358  LockGuard lg(mutex[MUTEX_IMU]);
359  Vector _imu=imu;
360  return _imu;
361 }
362 
363 
364 /************************************************************************/
366 {
367  ostringstream str;
368  str<<"v"<<head_version;
369  return str.str();
370 }
371 
372 
373 /************************************************************************/
374 IMUPort::IMUPort() : commData(NULL)
375 {
376  useCallback();
377 }
378 
379 
380 /************************************************************************/
382 {
383  this->commData=commData;
384 }
385 
386 
387 /************************************************************************/
388 void IMUPort::onRead(Vector &imu)
389 {
390  if (commData!=NULL)
391  commData->set_imu(imu);
392 }
393 
394 
395 /************************************************************************/
396 bool GazeComponent::getExtrinsicsMatrix(const string &type, Matrix &M)
397 {
398  if (type=="left")
399  {
400  M=eyeL->asChain()->getHN();
401  return true;
402  }
403  else if (type=="right")
404  {
405  M=eyeR->asChain()->getHN();
406  return true;
407  }
408  else
409  return false;
410 }
411 
412 
413 /************************************************************************/
414 bool GazeComponent::setExtrinsicsMatrix(const string &type, const Matrix &M)
415 {
416  if (type=="left")
417  {
418  eyeL->asChain()->setHN(M);
419  return true;
420  }
421  else if (type=="right")
422  {
423  eyeR->asChain()->setHN(M);
424  return true;
425  }
426  else
427  return false;
428 }
429 
430 
431 /************************************************************************/
432 bool getCamParams(const ResourceFinder &rf, const string &type,
433  Matrix **Prj, int &w, int &h, const bool verbose)
434 {
435  ResourceFinder &_rf=const_cast<ResourceFinder&>(rf);
436  *Prj=NULL;
437 
438  if (!_rf.isConfigured())
439  return false;
440 
441  string message=_rf.findFile("from");
442  if (!message.empty())
443  {
444  message+=": intrinsic parameters for "+type;
445  Bottle &parType=_rf.findGroup(type);
446  if (parType.check("w") && parType.check("h") &&
447  parType.check("fx") && parType.check("fy") &&
448  parType.check("cx") && parType.check("cy"))
449  {
450  w=parType.find("w").asInt();
451  h=parType.find("h").asInt();
452  double fx=parType.find("fx").asDouble();
453  double fy=parType.find("fy").asDouble();
454  double cx=parType.find("cx").asDouble();
455  double cy=parType.find("cy").asDouble();
456 
457  if (verbose)
458  {
459  yInfo("%s found:",message.c_str());
460  yInfo("w = %d",w);
461  yInfo("h = %d",h);
462  yInfo("fx = %g",fx);
463  yInfo("fy = %g",fy);
464  yInfo("cx = %g",cx);
465  yInfo("cy = %g",cy);
466  }
467 
468  *Prj=new Matrix(eye(3,4));
469 
470  Matrix &K=**Prj;
471  K(0,0)=fx; K(1,1)=fy;
472  K(0,2)=cx; K(1,2)=cy;
473 
474  return true;
475  }
476  }
477  else
478  {
479  message=_rf.find("from").asString();
480  message+=": intrinsic parameters for "+type;
481  }
482 
483  if (verbose)
484  yWarning("%s not found!",message.c_str());
485 
486  return false;
487 }
488 
489 
490 /************************************************************************/
491 bool getAlignHN(const ResourceFinder &rf, const string &type,
492  iKinChain *chain, const bool verbose)
493 {
494  ResourceFinder &_rf=const_cast<ResourceFinder&>(rf);
495  if ((chain!=NULL) && _rf.isConfigured())
496  {
497  string message=_rf.findFile("from");
498  if (!message.empty())
499  {
500  message+=": aligning matrix for "+type;
501  Bottle &parType=_rf.findGroup(type);
502  if (Bottle *bH=parType.find("HN").asList())
503  {
504  int i=0;
505  int j=0;
506 
507  Matrix HN(4,4); HN=0.0;
508  for (int cnt=0; (cnt<bH->size()) && (cnt<HN.rows()*HN.cols()); cnt++)
509  {
510  HN(i,j)=bH->get(cnt).asDouble();
511  if (++j>=HN.cols())
512  {
513  i++;
514  j=0;
515  }
516  }
517 
518  // enforce the homogeneous property
519  HN(3,0)=HN(3,1)=HN(3,2)=0.0;
520  HN(3,3)=1.0;
521 
522  chain->setHN(HN);
523 
524  if (verbose)
525  {
526  yInfo("%s found:",message.c_str());
527  yInfo("%s",HN.toString(3,3).c_str());
528  }
529 
530  return true;
531  }
532  }
533  else
534  {
535  message=_rf.find("from").asString();
536  message+=": aligning matrix for "+type;
537  }
538 
539  if (verbose)
540  yWarning("%s not found!",message.c_str());
541  }
542 
543  return false;
544 }
545 
546 
547 /************************************************************************/
548 Matrix alignJointsBounds(iKinChain *chain, PolyDriver *drvTorso,
549  PolyDriver *drvHead, const ExchangeData *commData)
550 {
551  IEncoders *encs;
552  IControlLimits *lims;
553 
554  double min, max;
555  int nJointsTorso=3;
556 
557  if (drvTorso!=NULL)
558  {
559  drvTorso->view(encs);
560  drvTorso->view(lims);
561  encs->getAxes(&nJointsTorso);
562 
563  for (int i=0; i<nJointsTorso; i++)
564  {
565  if (lims->getLimits(i,&min,&max))
566  {
567  if (commData->head_version<3.0)
568  {
569  (*chain)[nJointsTorso-1-i].setMin(CTRL_DEG2RAD*min);
570  (*chain)[nJointsTorso-1-i].setMax(CTRL_DEG2RAD*max);
571  }
572  else
573  {
574  (*chain)[i].setMin(CTRL_DEG2RAD*min);
575  (*chain)[i].setMax(CTRL_DEG2RAD*max);
576  }
577  }
578  else
579  yError("unable to retrieve limits for torso joint #%d",i);
580  }
581  }
582 
583  drvHead->view(encs);
584  drvHead->view(lims);
585  int nJointsHead;
586  encs->getAxes(&nJointsHead);
587  Matrix lim(nJointsHead,2);
588 
589  for (int i=0; i<nJointsHead; i++)
590  {
591  if (lims->getLimits(i,&min,&max))
592  {
593  // limit eye's tilt due to eyelids
594  if (i==3)
595  {
596  min=std::max(min,commData->eyeTiltLim[0]);
597  max=std::min(max,commData->eyeTiltLim[1]);
598  }
599 
600  lim(i,0)=CTRL_DEG2RAD*min;
601  lim(i,1)=CTRL_DEG2RAD*max;
602 
603  // just one eye's got only 5 dofs
604  if (i<nJointsHead-1)
605  {
606  (*chain)[nJointsTorso+i].setMin(lim(i,0));
607  (*chain)[nJointsTorso+i].setMax(lim(i,1));
608  }
609  }
610  else
611  yError("unable to retrieve limits for head joint #%d",i);
612  }
613 
614  return lim;
615 }
616 
617 
618 /************************************************************************/
620 {
621  unsigned int N1=ch1->getN();
622  unsigned int N2=ch2->getN();
623  unsigned int N =N1>N2 ? N2 : N1;
624 
625  for (unsigned int i=0; i<N; i++)
626  {
627  (*ch2)[i].setMin((*ch1)[i].getMin());
628  (*ch2)[i].setMax((*ch1)[i].getMax());
629  }
630 }
631 
632 
633 /************************************************************************/
634 void updateTorsoBlockedJoints(iKinChain *chain, const Vector &fbTorso)
635 {
636  for (unsigned int i=0; i<(unsigned int)fbTorso.length(); i++)
637  chain->setBlockingValue(i,fbTorso[i]);
638 }
639 
640 
641 /************************************************************************/
642 void updateNeckBlockedJoints(iKinChain *chain, const Vector &fbNeck)
643 {
644  for (int i=0; i<3; i++)
645  chain->setBlockingValue(3+i,fbNeck[i]);
646 }
647 
648 
649 /************************************************************************/
650 bool getFeedback(Vector &fbTorso, Vector &fbHead, PolyDriver *drvTorso,
651  PolyDriver *drvHead, const ExchangeData *commData,
652  double *timeStamp)
653 {
654  IEncodersTimed *encs;
655 
656  int nJointsTorso=(int)fbTorso.length();
657  int nJointsHead=(int)fbHead.length();
658 
659  Vector fb(std::max(nJointsTorso,nJointsHead));
660  Vector stamps(nJointsTorso+nJointsHead,0.0);
661  bool ret=true;
662 
663  if (drvTorso!=NULL)
664  {
665  drvTorso->view(encs);
666  if (encs->getEncodersTimed(fb.data(),stamps.data()))
667  {
668  for (int i=0; i<nJointsTorso; i++)
669  fbTorso[i]=CTRL_DEG2RAD*((commData->head_version<3.0)?fb[nJointsTorso-1-i]:fb[i]);
670  }
671  else
672  ret=false;
673  }
674  else
675  fbTorso=0.0;
676 
677  drvHead->view(encs);
678  if (encs->getEncodersTimed(fb.data(),stamps.data()+nJointsTorso))
679  {
680  for (int i=0; i<nJointsHead; i++)
681  fbHead[i]=CTRL_DEG2RAD*fb[i];
682  }
683  else
684  ret=false;
685 
686  // impose vergence != 0.0
687  if (commData!=NULL)
688  fbHead[nJointsHead-1]=std::max(fbHead[nJointsHead-1],commData->minAllowedVergence);
689 
690  // retrieve the highest encoders time stamp
691  if (timeStamp!=NULL)
692  *timeStamp=findMax(stamps);
693 
694  return ret;
695 }
696 
697 
#define MUTEX_XD
Definition: utils.cpp:27
Event triggerNeck
Definition: utils.h:59
bool locked
Definition: utils.h:64
IMUPort()
Definition: utils.cpp:374
string headVersion2String()
Definition: utils.cpp:365
void copyJointsBounds(iKinChain *ch1, iKinChain *ch2)
Definition: utils.cpp:619
Vector xd
Definition: utils.h:60
#define MUTEX_TORSO
Definition: utils.cpp:31
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:579
bool closing
Definition: utils.h:65
void set_v(const Vector &_v)
Definition: utils.cpp:242
Matrix alignJointsBounds(iKinChain *chain, PolyDriver *drvTorso, PolyDriver *drvHead, const ExchangeData *commData)
Definition: utils.cpp:548
static int stop
Definition: iCub_Sim.cpp:46
void updateTorsoBlockedJoints(iKinChain *chain, const Vector &fbTorso)
Definition: utils.cpp:634
void resize_v(const int sz, const double val)
Definition: utils.cpp:169
void set_counterv(const Vector &_counterv)
Definition: utils.cpp:250
#define SACCADES_ACTIVATION_ANGLE
Definition: solver.h:39
void resize_counterv(const int sz, const double val)
Definition: utils.cpp:177
void set_qd(const Vector &_qd)
Definition: utils.cpp:193
bool set_xd(const Vector &_xd)
Definition: utils.cpp:86
bool getAlignHN(const ResourceFinder &rf, const string &type, iKinChain *chain, const bool verbose)
Definition: utils.cpp:491
Matrix get_fpFrame()
Definition: utils.cpp:347
A Base class for defining a Serial Link Chain.
Definition: iKinFwd.h:354
xdPort(void *_slv)
Definition: utils.cpp:39
Vector get_q()
Definition: utils.cpp:311
int n
#define MUTEX_IMU
Definition: utils.cpp:35
Vector get_imu()
Definition: utils.cpp:356
Vector eyeTiltLim
Definition: utils.h:136
void run()
Definition: utils.cpp:119
void set_torso(const Vector &_torso)
Definition: utils.cpp:234
#define CTRL_DEG2RAD
Definition: XSensMTx.cpp:25
const FSC max
Definition: strain.h:48
static int v
Definition: iCub_Sim.cpp:47
Vector get_counterv()
Definition: utils.cpp:338
Vector get_v()
Definition: utils.cpp:329
void set_x(const Vector &_x)
Definition: utils.cpp:209
void set_xd(const Vector &_xd)
Definition: utils.cpp:185
bool setBlockingValue(const unsigned int i, double Ang)
Changes the value of the ith blocked Link.
Definition: iKinFwd.cpp:413
Mutex mutex_0
Definition: utils.h:57
const FSC min
Definition: strain.h:49
void onRead(Vector &imu)
Definition: utils.cpp:388
#define MUTEX_Q
Definition: utils.cpp:30
Vector get_xd()
Definition: utils.cpp:101
GtkWidget * message
Definition: main.cpp:185
virtual bool setExtrinsicsMatrix(const string &type, const Matrix &M)
Definition: utils.cpp:414
Vector get_xd()
Definition: utils.cpp:274
#define SACCADES_INHIBITION_PERIOD
Definition: solver.h:38
#define MUTEX_QD
Definition: utils.cpp:28
#define MUTEX_V
Definition: utils.cpp:32
void init(const Vector &xd0)
Definition: utils.cpp:52
void set_q(const Vector &_q)
Definition: utils.cpp:226
Mutex mutex_1
Definition: utils.h:58
~xdPort()
Definition: utils.cpp:59
#define NECKSOLVER_ACTIVATIONDELAY
Definition: solver.h:40
Vector get_x()
Definition: utils.cpp:292
Definition: solver.h:118
#define MUTEX_X
Definition: utils.cpp:29
void * slv
Definition: utils.h:55
ExchangeData * commData
Definition: utils.h:164
ExchangeData()
Definition: utils.cpp:141
bool isNew
Definition: utils.h:62
bool getCamParams(const ResourceFinder &rf, const string &type, Matrix **Prj, int &w, int &h, const bool verbose)
Definition: utils.cpp:432
bool getFeedback(Vector &fbTorso, Vector &fbHead, PolyDriver *drvTorso, PolyDriver *drvHead, const ExchangeData *commData, double *timeStamp)
Definition: utils.cpp:650
Vector xdDelayed
Definition: utils.h:61
Vector get_xdDelayed()
Definition: utils.cpp:110
Vector get_torso()
Definition: utils.cpp:320
void set_imu(const Vector &_imu)
Definition: utils.cpp:266
unsigned int getN() const
Returns the number of Links belonging to the Chain.
Definition: iKinFwd.h:550
void setExchangeData(ExchangeData *commData)
Definition: utils.cpp:381
#define MUTEX_COUNTERV
Definition: utils.cpp:33
void onRead(Bottle &b)
Definition: utils.cpp:68
void set_fpFrame(const Matrix &_S)
Definition: utils.cpp:258
bool isNewDelayed
Definition: utils.h:63
int rx
Definition: utils.h:66
#define NECKSOLVER_RESTORINGANGLE
Definition: solver.h:43
#define MUTEX_FPFRAME
Definition: utils.cpp:34
Vector get_qd()
Definition: utils.cpp:283
double head_version
Definition: utils.h:141
virtual bool getExtrinsicsMatrix(const string &type, Matrix &M)
Definition: utils.cpp:396
void updateNeckBlockedJoints(iKinChain *chain, const Vector &fbNeck)
Definition: utils.cpp:642
double minAllowedVergence
Definition: utils.h:137