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