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