iCub-main
iKinFwd.cpp
Go to the documentation of this file.
1 /*
2  * Copyright (C) 2006-2018 Istituto Italiano di Tecnologia (IIT)
3  * Copyright (C) 2006-2010 RobotCub Consortium
4  * All rights reserved.
5  *
6  * This software may be modified and distributed under the terms
7  * of the BSD-3-Clause license. See the accompanying LICENSE file for
8  * details.
9 */
10 
11 #include <cstdlib>
12 #include <sstream>
13 #include <cmath>
14 #include <algorithm>
15 
16 #include <yarp/os/Log.h>
17 
18 #include <iCub/iKin/iKinFwd.h>
19 
20 using namespace std;
21 using namespace yarp::os;
22 using namespace yarp::dev;
23 using namespace yarp::sig;
24 using namespace yarp::math;
25 using namespace iCub::ctrl;
26 using namespace iCub::iKin;
27 
28 namespace iCub {
29  namespace iKin {
30  constexpr double version_epsilon = 1e-6;
31  inline bool is_version(const double v1, const double v2) {
32  return (fabs(v1 - v2) < version_epsilon);
33  }
34  }
35 }
36 
37 
38 /************************************************************************/
39 void iCub::iKin::notImplemented(const unsigned int verbose)
40 {
41  if (verbose)
42  yError("iKin: not implemented");
43 }
44 
45 
46 /************************************************************************/
47 iKinLink::iKinLink(double _A, double _D, double _Alpha, double _Offset,
48  double _Min, double _Max): zeros1x1(zeros(1,1)), zeros1(zeros(1))
49 {
50  A =_A;
51  D =_D;
52  Alpha =_Alpha;
53  Offset=_Offset;
54 
55  Min=_Min;
56  Max=_Max;
57  Ang=Min;
58 
59  c_alpha=cos(Alpha);
60  s_alpha=sin(Alpha);
61 
62  blocked =false;
63  cumulative =false;
64  constrained=true;
65  verbose =0;
66 
67  H.resize(4,4);
68  H.zero();
69  DnH =H;
70  cumH=H;
71  cumH.eye();
72 
73  H(2,1)=s_alpha;
74  H(2,2)=c_alpha;
75  H(2,3)=D;
76  H(3,3)=1.0;
77 }
78 
79 
80 /************************************************************************/
81 void iKinLink::clone(const iKinLink &l)
82 {
83  A =l.A;
84  D =l.D;
85  Alpha =l.Alpha;
86  Offset=l.Offset;
87 
88  c_alpha=l.c_alpha;
89  s_alpha=l.s_alpha;
90 
91  Ang=l.Ang;
92  Min=l.Min;
93  Max=l.Max;
94 
95  blocked =l.blocked;
98  verbose =l.verbose;
99 
100  H =l.H;
101  cumH=l.cumH;
102  DnH =l.DnH;
103 }
104 
105 
106 /************************************************************************/
108 {
109  clone(l);
110 }
111 
112 
113 /************************************************************************/
115 {
116  clone(l);
117 
118  return *this;
119 }
120 
121 
122 /************************************************************************/
123 void iKinLink::setMin(const double _Min)
124 {
125  Min=_Min;
126 
127  if (Ang<Min)
128  Ang=Min;
129 }
130 
131 
132 /************************************************************************/
133 void iKinLink::setMax(const double _Max)
134 {
135  Max=_Max;
136 
137  if (Ang>Max)
138  Ang=Max;
139 }
140 
141 
142 /************************************************************************/
143 void iKinLink::setD(const double _D)
144 {
145  H(2,3)=D=_D;
146 }
147 
148 
149 /************************************************************************/
150 void iKinLink::setAlpha(const double _Alpha)
151 {
152  Alpha=_Alpha;
153 
154  H(2,2)=c_alpha=cos(Alpha);
155  H(2,1)=s_alpha=sin(Alpha);
156 }
157 
158 
159 /************************************************************************/
160 double iKinLink::setAng(double _Ang)
161 {
162  if (!blocked)
163  {
164  if (constrained)
165  Ang=(_Ang<Min) ? Min : ((_Ang>Max) ? Max : _Ang);
166  else
167  Ang=_Ang;
168  }
169  else if (verbose)
170  yWarning("Attempt to set joint angle to %g while blocked",_Ang);
171 
172  return Ang;
173 }
174 
175 
176 /************************************************************************/
177 Matrix iKinLink::getH(bool c_override)
178 {
179  double theta=Ang+Offset;
180  double c_theta=cos(theta);
181  double s_theta=sin(theta);
182 
183  H(0,0)=c_theta;
184  H(0,1)=-s_theta*c_alpha;
185  H(0,2)=s_theta*s_alpha;
186  H(0,3)=c_theta*A;
187 
188  H(1,0)=s_theta;
189  H(1,1)=c_theta*c_alpha;
190  H(1,2)=-c_theta*s_alpha;
191  H(1,3)=s_theta*A;
192 
193  if (cumulative && !c_override)
194  return cumH*H;
195  else
196  return H;
197 }
198 
199 
200 /************************************************************************/
201 Matrix iKinLink::getH(double _Ang, bool c_override)
202 {
203  setAng(_Ang);
204 
205  return getH(c_override);
206 }
207 
208 
209 /************************************************************************/
210 Matrix iKinLink::getDnH(unsigned int n, bool c_override)
211 {
212  if (n==0)
213  return getH(c_override);
214  else
215  {
216  double theta=Ang+Offset;
217  double c_theta=cos(theta);
218  double s_theta=sin(theta);
219 
220  int C=(n>>1)&1 ? -1 : 1;
221 
222  if (n&1)
223  {
224  DnH(0,0)=-C*s_theta;
225  DnH(0,1)=-C*c_theta*c_alpha;
226  DnH(0,2)=C*c_theta*s_alpha;
227  DnH(0,3)=-C*s_theta*A;
228 
229  DnH(1,0)=C*c_theta;
230  DnH(1,1)=-C*s_theta*c_alpha;
231  DnH(1,2)=C*s_theta*s_alpha;
232  DnH(1,3)=C*c_theta*A;
233  }
234  else
235  {
236  DnH(0,0)=C*c_theta;
237  DnH(0,1)=-C*s_theta*c_alpha;
238  DnH(0,2)=C*s_theta*s_alpha;
239  DnH(0,3)=C*c_theta*A;
240 
241  DnH(1,0)=C*s_theta;
242  DnH(1,1)=C*c_theta*c_alpha;
243  DnH(1,2)=-C*c_theta*s_alpha;
244  DnH(1,3)=C*s_theta*A;
245  }
246 
247  if (cumulative && !c_override)
248  return cumH*DnH;
249  else
250  return DnH;
251  }
252 }
253 
254 
255 /************************************************************************/
256 void iKinLink::addCumH(const Matrix &_cumH)
257 {
258  cumulative=true;
259  cumH=_cumH;
260 }
261 
262 
263 /************************************************************************/
265 {
266  N=DOF=verbose=0;
267  H0=HN=eye(4,4);
268 }
269 
270 
271 /************************************************************************/
273 {
274  N =c.N;
275  DOF =c.DOF;
276  H0 =c.H0;
277  HN =c.HN;
278  curr_q =c.curr_q;
279  verbose =c.verbose;
280  hess_J =c.hess_J;
282 
283  allList.assign(c.allList.begin(),c.allList.end());
284  quickList.assign(c.quickList.begin(),c.quickList.end());
285  hash.assign(c.hash.begin(),c.hash.end());
286  hash_dof.assign(c.hash_dof.begin(),c.hash_dof.end());
287 }
288 
289 
290 /************************************************************************/
292 {
293  clone(c);
294 }
295 
296 
297 /************************************************************************/
299 {
300  clone(c);
301 
302  return *this;
303 }
304 
305 
306 /************************************************************************/
307 bool iKinChain::addLink(const unsigned int i, iKinLink &l)
308 {
309  if (i<=N)
310  {
311  allList.insert(allList.begin()+i,&l);
312  N=(unsigned int)allList.size();
313 
314  build();
315 
316  return true;
317  }
318  else
319  {
320  if (verbose)
321  yError("addLink() failed due to out of range index: %d>%d",i,N);
322 
323  return false;
324  }
325 }
326 
327 
328 /************************************************************************/
329 bool iKinChain::rmLink(const unsigned int i)
330 {
331  if (i<N)
332  {
333  allList.erase(allList.begin()+i);
334  N=(unsigned int)allList.size();
335 
336  build();
337 
338  return true;
339  }
340  else
341  {
342  if (verbose)
343  yError("rmLink() failed due to out of range index: %d>=%d",i,N);
344 
345  return false;
346  }
347 }
348 
349 
350 /************************************************************************/
352 {
353  allList.push_back(&l);
354  N=(unsigned int)allList.size();
355 
356  build();
357 }
358 
359 
360 /************************************************************************/
362 {
363  allList.clear();
364  quickList.clear();
365  hash.clear();
366  hash_dof.clear();
367 
368  N=DOF=0;
369  H0=HN=eye(4,4);
370 }
371 
372 
373 /************************************************************************/
375 {
376  pushLink(l);
377 
378  return *this;
379 }
380 
381 
382 /************************************************************************/
384 {
385  allList.pop_back();
386  N=(unsigned int)allList.size();
387 
388  build();
389 }
390 
391 
392 /************************************************************************/
394 {
395  popLink();
396 
397  return *this;
398 }
399 
400 
401 /************************************************************************/
402 bool iKinChain::blockLink(const unsigned int i, double Ang)
403 {
404  if (i<N)
405  {
406  allList[i]->block(Ang);
407  build();
408 
409  return true;
410  }
411  else
412  {
413  if (verbose)
414  yError("blockLink() failed due to out of range index: %d>=%d",i,N);
415 
416  return false;
417  }
418 }
419 
420 
421 /************************************************************************/
422 bool iKinChain::setBlockingValue(const unsigned int i, double Ang)
423 {
424  if (i<N)
425  {
426  if (allList[i]->isBlocked() && (Ang!=allList[i]->getAng()))
427  {
428  allList[i]->blocked=false; // remove the block temporarly
429  allList[i]->block(Ang); // update the blocked link
430 
431  // update the cumulative link which follows in the chain
432  if (i<N-1)
433  {
434  Matrix H=eye(4,4);
435  int j;
436 
437  for (j=i-1; j>=0; j--)
438  if (!allList[j]->isBlocked())
439  break;
440 
441  for (++j; j<=(int)i; j++)
442  H*=allList[j]->getH(true);
443 
444  for (; j<(int)N && !allList[j]->isCumulative(); j++)
445  H*=allList[j]->getH(true);
446 
447  allList[j]->addCumH(H);
448  }
449 
450  return true;
451  }
452  else
453  {
454  if (verbose)
455  yError("setBlockingValue() failed since the %dth link was not already blocked",i);
456 
457  return false;
458  }
459  }
460  else
461  {
462  if (verbose)
463  yError("setBlockingValue() failed due to out of range index: %d>=%d",i,N);
464 
465  return false;
466  }
467 }
468 
469 
470 /************************************************************************/
471 bool iKinChain::releaseLink(const unsigned int i)
472 {
473  if (i<N)
474  {
475  allList[i]->release();
476  build();
477 
478  return true;
479  }
480  else
481  {
482  if (verbose)
483  yError("releaseLink() failed due to out of range index: %d>=%d",i,N);
484 
485  return false;
486  }
487 }
488 
489 
490 /************************************************************************/
491 bool iKinChain::isLinkBlocked(const unsigned int i)
492 {
493  if (i<N)
494  return allList[i]->isBlocked();
495  else
496  {
497  if (verbose)
498  yError("isLinkBlocked() failed due to out of range index: %d>=%d",i,N);
499 
500  return false;
501  }
502 }
503 
504 
505 /************************************************************************/
506 void iKinChain::setAllConstraints(bool _constrained)
507 {
508  for (unsigned int i=0; i<N; i++)
509  allList[i]->setConstraint(_constrained);
510 }
511 
512 
513 /************************************************************************/
514 void iKinChain::setAllLinkVerbosity(unsigned int _verbose)
515 {
516  for (unsigned int i=0; i<N; i++)
517  allList[i]->setVerbosity(_verbose);
518 }
519 
520 
521 /************************************************************************/
523 {
524  quickList.clear();
525  hash.clear();
526  hash_dof.clear();
527  DOF=0;
528 
529  Matrix H=eye(4,4);
530  bool cumulOn=false;
531 
532  for (unsigned int i=0; i<N; i++)
533  {
534  allList[i]->rmCumH();
535 
536  if (allList[i]->isBlocked())
537  {
538  if (i==N-1)
539  {
540  allList[i]->addCumH(H);
541  quickList.push_back(allList[i]);
542  }
543  else
544  {
545  H*=allList[i]->getH();
546  cumulOn=true;
547  }
548  }
549  else
550  {
551  if (cumulOn)
552  allList[i]->addCumH(H);
553 
554  DOF++;
555  quickList.push_back(allList[i]);
556  hash_dof.push_back((unsigned int)quickList.size()-1);
557  hash.push_back(i);
558 
559  H.eye();
560  cumulOn=false;
561  }
562  }
563 
564  if (DOF>0)
565  curr_q.resize(DOF,0);
566 }
567 
568 
569 /************************************************************************/
570 bool iKinChain::setH0(const Matrix &_H0)
571 {
572  if ((_H0.rows()==4) && (_H0.cols()==4))
573  {
574  H0=_H0;
575  return true;
576  }
577  else
578  {
579  if (verbose)
580  yError("Attempt to reference a wrong matrix H0 (not 4x4)");
581 
582  return false;
583  }
584 }
585 
586 
587 /************************************************************************/
588 bool iKinChain::setHN(const Matrix &_HN)
589 {
590  if ((_HN.rows()==4) && (_HN.cols()==4))
591  {
592  HN=_HN;
593  return true;
594  }
595  else
596  {
597  if (verbose)
598  yError("Attempt to reference a wrong matrix HN (not 4x4)");
599 
600  return false;
601  }
602 }
603 
604 
605 /************************************************************************/
606 Vector iKinChain::setAng(const Vector &q)
607 {
608  yAssert(DOF>0);
609 
610  size_t sz=std::min(q.length(),(size_t)DOF);
611  for (size_t i=0; i<sz; i++)
612  curr_q[i]=quickList[hash_dof[i]]->setAng(q[i]);
613 
614  return curr_q;
615 }
616 
617 
618 /************************************************************************/
620 {
621  yAssert(DOF>0);
622 
623  for (unsigned int i=0; i<DOF; i++)
624  curr_q[i]=quickList[hash_dof[i]]->getAng();
625 
626  return curr_q;
627 }
628 
629 
630 /************************************************************************/
631 double iKinChain::setAng(const unsigned int i, double _Ang)
632 {
633  double res=0.0;
634 
635  if (i<N)
636  {
637  if (allList[i]->isBlocked())
638  {
639  setBlockingValue(i,_Ang);
640  res=allList[i]->getAng();
641  }
642  else
643  res=allList[i]->setAng(_Ang);
644  }
645  else if (verbose)
646  yError("setAng() failed due to out of range index: %d>=%d",i,N);
647 
648  return res;
649 }
650 
651 
652 /************************************************************************/
653 double iKinChain::getAng(const unsigned int i)
654 {
655  double res=0.0;
656 
657  if (i<N)
658  res=allList[i]->getAng();
659  else if (verbose)
660  yError("getAng() failed due to out of range index: %d>=%d",i,N);
661 
662  return res;
663 }
664 
665 
666 /************************************************************************/
667 Vector iKinChain::RotAng(const Matrix &R)
668 {
669  Vector r(3);
670 
671  // Euler Angles as XYZ (see dcm2angle.m)
672  r[0]=atan2(-R(2,1),R(2,2));
673  r[1]=asin(R(2,0));
674  r[2]=atan2(-R(1,0),R(0,0));
675 
676  return r;
677 }
678 
679 
680 /************************************************************************/
681 Vector iKinChain::dRotAng(const Matrix &R, const Matrix &dR)
682 {
683  Vector dr(3);
684 
685  dr[0]=(R(2,1)*dR(2,2) - R(2,2)*dR(2,1)) / (R(2,1)*R(2,1) + R(2,2)*R(2,2));
686  dr[1]=dR(2,0)/sqrt(fabs(1-R(2,0)*R(2,0)));
687  dr[2]=(R(1,0)*dR(0,0) - R(0,0)*dR(1,0)) / (R(1,0)*R(1,0) + R(0,0)*R(0,0));
688 
689  return dr;
690 }
691 
692 
693 /************************************************************************/
694 Vector iKinChain::d2RotAng(const Matrix &R, const Matrix &dRi,
695  const Matrix &dRj, const Matrix &d2R)
696 {
697  Vector d2r(3);
698 
699  double y,yi,yj,yij,x,xi,xj,xij;
700  double tmp1,tmp2;
701 
702  y =-R(2,1);
703  yi =-dRi(2,1);
704  yj =-dRj(2,1);
705  yij=-d2R(2,1);
706  x = R(2,2);
707  xi = dRi(2,2);
708  xj = dRj(2,2);
709  xij= d2R(2,2);
710 
711  tmp1 =x*x+y*y;
712  d2r[0]=((xj*yi+x*yij-xij*y-xi*yj)*tmp1 - 2.0*(x*yi-xi*y)*(x*xj+y*yj)) / (tmp1*tmp1);
713 
714  x =R(2,0);
715  xi =dRi(2,0);
716  xj =dRj(2,0);
717  xij=d2R(2,0);
718 
719  tmp1 =1-x*x;
720  tmp2 =sqrt(fabs(tmp1));
721  d2r[1]=(xij-(x*xi*xj)/tmp1) / (tmp1*tmp2);
722 
723  y =-R(1,0);
724  yi =-dRi(1,0);
725  yj =-dRj(1,0);
726  yij=-d2R(1,0);
727  x = R(0,0);
728  xi = dRi(0,0);
729  xj = dRj(0,0);
730  xij= d2R(0,0);
731 
732  tmp1 =x*x+y*y;
733  d2r[2]=((xj*yi+x*yij-xij*y-xi*yj)*tmp1 - 2.0*(x*yi-xi*y)*(x*xj+y*yj)) / (tmp1*tmp1);
734 
735  return d2r;
736 }
737 
738 
739 /************************************************************************/
740 Matrix iKinChain::getH(const unsigned int i, const bool allLink)
741 {
742  Matrix H=H0;
743  unsigned int _i,n;
744  deque<iKinLink*> *l;
745  bool cumulHN=false;
746  bool c_override;
747 
748  if (allLink)
749  {
750  n=N;
751  l=&allList;
752  c_override=true;
753 
754  _i=i;
755  if (_i>=N-1)
756  cumulHN=true;
757  }
758  else
759  {
760  n=DOF;
761  l=&quickList;
762  c_override=false;
763 
764  if (i==DOF)
765  _i=(unsigned int)quickList.size();
766  else
767  _i=i;
768 
769  if (hash[_i]>=N-1)
770  cumulHN=true;
771  }
772 
773  yAssert(i<n);
774 
775  for (unsigned int j=0; j<=_i; j++)
776  H*=((*l)[j]->getH(c_override));
777 
778  if (cumulHN)
779  H*=HN;
780 
781  return H;
782 }
783 
784 
785 /************************************************************************/
787 {
788  // may be different from DOF since one blocked link may lie
789  // at the end of the chain.
790  unsigned int n=(unsigned int)quickList.size();
791  Matrix H=H0;
792 
793  for (unsigned int i=0; i<n; i++)
794  H*=quickList[i]->getH();
795 
796  return H*HN;
797 }
798 
799 
800 /************************************************************************/
801 Matrix iKinChain::getH(const Vector &q)
802 {
803  yAssert(DOF>0);
804 
805  setAng(q);
806  return getH();
807 }
808 
809 
810 /************************************************************************/
811 Vector iKinChain::Pose(const unsigned int i, const bool axisRep)
812 {
813  Matrix H=getH(i,true);
814  Vector v;
815 
816  if (i<N)
817  {
818  if (axisRep)
819  {
820  v.resize(7);
821  Vector r=dcm2axis(H);
822  v[0]=H(0,3);
823  v[1]=H(1,3);
824  v[2]=H(2,3);
825  v[3]=r[0];
826  v[4]=r[1];
827  v[5]=r[2];
828  v[6]=r[3];
829  }
830  else
831  {
832  v.resize(6);
833  Vector r=RotAng(H);
834  v[0]=H(0,3);
835  v[1]=H(1,3);
836  v[2]=H(2,3);
837  v[3]=r[0];
838  v[4]=r[1];
839  v[5]=r[2];
840  }
841  }
842  else if (verbose)
843  yError("Pose() failed due to out of range index: %d>=%d",i,N);
844 
845  return v;
846 }
847 
848 
849 /************************************************************************/
850 Vector iKinChain::Position(const unsigned int i)
851 {
852  yAssert(i<N);
853  return getH(i,true).subcol(0,3,3);
854 }
855 
856 
857 /************************************************************************/
858 Vector iKinChain::EndEffPose(const bool axisRep)
859 {
860  Matrix H=getH();
861  Vector v;
862 
863  if (axisRep)
864  {
865  v.resize(7);
866  Vector r=dcm2axis(H);
867  v[0]=H(0,3);
868  v[1]=H(1,3);
869  v[2]=H(2,3);
870  v[3]=r[0];
871  v[4]=r[1];
872  v[5]=r[2];
873  v[6]=r[3];
874  }
875  else
876  {
877  v.resize(6);
878  Vector r=RotAng(H);
879  v[0]=H(0,3);
880  v[1]=H(1,3);
881  v[2]=H(2,3);
882  v[3]=r[0];
883  v[4]=r[1];
884  v[5]=r[2];
885  }
886 
887  return v;
888 }
889 
890 
891 /************************************************************************/
892 Vector iKinChain::EndEffPose(const Vector &q, const bool axisRep)
893 {
894  yAssert(DOF>0);
895 
896  setAng(q);
897  return EndEffPose(axisRep);
898 }
899 
900 
901 /************************************************************************/
903 {
904  return getH().subcol(0,3,3);
905 }
906 
907 
908 /************************************************************************/
909 Vector iKinChain::EndEffPosition(const Vector &q)
910 {
911  yAssert(DOF>0);
912 
913  setAng(q);
914  return EndEffPosition();
915 }
916 
917 
918 /************************************************************************/
919 Matrix iKinChain::AnaJacobian(const unsigned int i, unsigned int col)
920 {
921  yAssert(i<N);
922 
923  col=col>3 ? 3 : col;
924 
925  Matrix J(6,i+1);
926  Matrix H,dH,_H;
927  Vector dr;
928 
929  for (unsigned int j=0; j<=i; j++)
930  {
931  H=dH=H0;
932 
933  for (unsigned int k=0; k<=i; k++)
934  {
935  _H=allList[k]->getH(true);
936  H*=_H;
937 
938  if (j==k)
939  dH*=allList[k]->getDnH(1,true);
940  else
941  dH*=_H;
942  }
943 
944  if (i>=N-1)
945  {
946  H*=HN;
947  dH*=HN;
948  }
949 
950  dr=dRotAng(H,dH);
951 
952  J(0,j)=dH(0,col);
953  J(1,j)=dH(1,col);
954  J(2,j)=dH(2,col);
955  J(3,j)=dr[0];
956  J(4,j)=dr[1];
957  J(5,j)=dr[2];
958  }
959 
960  return J;
961 }
962 
963 
964 /************************************************************************/
965 Matrix iKinChain::AnaJacobian(unsigned int col)
966 {
967  yAssert(DOF>0);
968 
969  col=col>3 ? 3 : col;
970 
971  // may be different from DOF since one blocked link may lie
972  // at the end of the chain.
973  unsigned int n=(unsigned int)quickList.size();
974  Matrix J(6,DOF);
975  Matrix H,dH,_H;
976  Vector dr;
977 
978  for (unsigned int i=0; i<DOF; i++)
979  {
980  H=dH=H0;
981 
982  for (unsigned int j=0; j<n; j++)
983  {
984  _H=quickList[j]->getH();
985  H*=_H;
986 
987  if (hash_dof[i]==j)
988  dH*=quickList[j]->getDnH();
989  else
990  dH*=_H;
991  }
992 
993  H*=HN;
994  dH*=HN;
995  dr=dRotAng(H,dH);
996 
997  J(0,i)=dH(0,col);
998  J(1,i)=dH(1,col);
999  J(2,i)=dH(2,col);
1000  J(3,i)=dr[0];
1001  J(4,i)=dr[1];
1002  J(5,i)=dr[2];
1003  }
1004 
1005  return J;
1006 }
1007 
1008 
1009 /************************************************************************/
1010 Matrix iKinChain::AnaJacobian(const Vector &q, unsigned int col)
1011 {
1012  yAssert(DOF>0);
1013 
1014  setAng(q);
1015  return AnaJacobian(col);
1016 }
1017 
1018 
1019 /************************************************************************/
1020 Matrix iKinChain::GeoJacobian(const unsigned int i)
1021 {
1022  yAssert(i<N);
1023 
1024  Matrix J(6,i+1);
1025  Matrix PN,Z;
1026  Vector w;
1027 
1028  deque<Matrix> intH;
1029  intH.push_back(H0);
1030 
1031  for (unsigned int j=0; j<=i; j++)
1032  intH.push_back(intH[j]*allList[j]->getH(true));
1033 
1034  PN=intH[i+1];
1035  if (i>=N-1)
1036  PN=PN*HN;
1037 
1038  for (unsigned int j=0; j<=i; j++)
1039  {
1040  Z=intH[j];
1041  w=cross(Z,2,PN-Z,3);
1042 
1043  J(0,j)=w[0];
1044  J(1,j)=w[1];
1045  J(2,j)=w[2];
1046  J(3,j)=Z(0,2);
1047  J(4,j)=Z(1,2);
1048  J(5,j)=Z(2,2);
1049  }
1050 
1051  return J;
1052 }
1053 
1054 
1055 /************************************************************************/
1057 {
1058  yAssert(DOF>0);
1059 
1060  Matrix J(6,DOF);
1061  Matrix PN,Z;
1062  Vector w;
1063 
1064  deque<Matrix> intH;
1065  intH.push_back(H0);
1066 
1067  for (unsigned int i=0; i<N; i++)
1068  intH.push_back(intH[i]*allList[i]->getH(true));
1069 
1070  PN=intH[N]*HN;
1071 
1072  for (unsigned int i=0; i<DOF; i++)
1073  {
1074  unsigned int j=hash[i];
1075 
1076  Z=intH[j];
1077  w=cross(Z,2,PN-Z,3);
1078 
1079  J(0,i)=w[0];
1080  J(1,i)=w[1];
1081  J(2,i)=w[2];
1082  J(3,i)=Z(0,2);
1083  J(4,i)=Z(1,2);
1084  J(5,i)=Z(2,2);
1085  }
1086 
1087  return J;
1088 }
1089 
1090 
1091 /************************************************************************/
1092 Matrix iKinChain::GeoJacobian(const Vector &q)
1093 {
1094  yAssert(DOF>0);
1095 
1096  setAng(q);
1097  return GeoJacobian();
1098 }
1099 
1100 
1101 /************************************************************************/
1102 Vector iKinChain::Hessian_ij(const unsigned int i, const unsigned int j)
1103 {
1105  return fastHessian_ij(i,j);
1106 }
1107 
1108 
1109 /************************************************************************/
1111 {
1112  if (DOF==0)
1113  {
1114  if (verbose)
1115  yError("prepareForHessian() failed since DOF==0");
1116 
1117  return;
1118  }
1119 
1120  hess_J=GeoJacobian();
1121 }
1122 
1123 
1124 /************************************************************************/
1125 Vector iKinChain::fastHessian_ij(const unsigned int i, const unsigned int j)
1126 {
1127  yAssert((i<DOF) && (j<DOF));
1128 
1129  // ref. E.D. Pohl, H. Lipkin, "A New Method of Robotic Motion Control Near Singularities",
1130  // Advanced Robotics, 1991
1131  Vector h(6,0.0);
1132  if(i<j)
1133  {
1134  //h.setSubvector(0,cross(hess_Jo,i,hess_Jl,j));
1135  h[0] = hess_J(4,i)*hess_J(2,j) - hess_J(5,i)*hess_J(1,j);
1136  h[1] = hess_J(5,i)*hess_J(0,j) - hess_J(3,i)*hess_J(2,j);
1137  h[2] = hess_J(3,i)*hess_J(1,j) - hess_J(4,i)*hess_J(0,j);
1138  //h.setSubvector(3,cross(hess_Jo,i,hess_Jo,j));
1139  h(3) = hess_J(4,i)*hess_J(5,j)-hess_J(5,i)*hess_J(4,j);
1140  h(4) = hess_J(5,i)*hess_J(3,j)-hess_J(3,i)*hess_J(5,j);
1141  h(5) = hess_J(3,i)*hess_J(4,j)-hess_J(4,i)*hess_J(3,j);
1142  }
1143  else
1144  {
1145  //h.setSubvector(0, cross(Jo,j,Jl,i));
1146  h[0] = hess_J(4,j)*hess_J(2,i) - hess_J(5,j)*hess_J(1,i);
1147  h[1] = hess_J(5,j)*hess_J(0,i) - hess_J(3,j)*hess_J(2,i);
1148  h[2] = hess_J(3,j)*hess_J(1,i) - hess_J(4,j)*hess_J(0,i);
1149  h[3]=h[4]=h[5]=0.0;
1150  }
1151 
1152  return h;
1153 }
1154 
1155 
1156 /************************************************************************/
1157 Vector iKinChain::Hessian_ij(const unsigned int lnk, const unsigned int i,
1158  const unsigned int j)
1159 {
1160  prepareForHessian(lnk);
1161  return fastHessian_ij(lnk,i,j);
1162 }
1163 
1164 
1165 /************************************************************************/
1166 void iKinChain::prepareForHessian(const unsigned int lnk)
1167 {
1168  if (lnk>=N)
1169  {
1170  if (verbose)
1171  yError("prepareForHessian() failed due to out of range index: %d>=%d",lnk,N);
1172 
1173  return;
1174  }
1175 
1176  hess_Jlnk=GeoJacobian(lnk);
1177 }
1178 
1179 
1180 /************************************************************************/
1181 Vector iKinChain::fastHessian_ij(const unsigned int lnk, const unsigned int i,
1182  const unsigned int j)
1183 {
1184  yAssert((i<lnk) && (j<lnk));
1185 
1186  Vector h(6,0.0);
1187  if (i<j)
1188  {
1189  //h.setSubvector(0,cross(hess_Jlnko,i,hess_Jlnkl,j));
1190  h[0] = hess_Jlnk(4,i)*hess_Jlnk(2,j) - hess_Jlnk(5,i)*hess_Jlnk(1,j);
1191  h[1] = hess_Jlnk(5,i)*hess_Jlnk(0,j) - hess_Jlnk(3,i)*hess_Jlnk(2,j);
1192  h[2] = hess_Jlnk(3,i)*hess_Jlnk(1,j) - hess_Jlnk(4,i)*hess_Jlnk(0,j);
1193  //h.setSubvector(3,cross(hess_Jlnko,i,hess_Jlnko,j));
1194  h[3] = hess_Jlnk(4,i)*hess_Jlnk(5,j) - hess_Jlnk(5,i)*hess_Jlnk(4,j);
1195  h[4] = hess_Jlnk(5,i)*hess_Jlnk(3,j) - hess_Jlnk(3,i)*hess_Jlnk(5,j);
1196  h[5] = hess_Jlnk(3,i)*hess_Jlnk(4,j) - hess_Jlnk(4,i)*hess_Jlnk(3,j);
1197  }
1198  else
1199  {
1200  //h.setSubvector(0,cross(hess_Jlnko,j,hess_Jlnkl,i));
1201  h[0] = hess_Jlnk(4,j)*hess_Jlnk(2,i) - hess_Jlnk(5,j)*hess_Jlnk(1,i);
1202  h[1] = hess_Jlnk(5,j)*hess_Jlnk(0,i) - hess_Jlnk(3,j)*hess_Jlnk(2,i);
1203  h[2] = hess_Jlnk(3,j)*hess_Jlnk(1,i) - hess_Jlnk(4,j)*hess_Jlnk(0,i);
1204  h[3]=h[4]=h[5]=0.0;
1205  }
1206 
1207  return h;
1208 }
1209 
1210 
1211 /************************************************************************/
1212 Matrix iKinChain::DJacobian(const Vector &dq)
1213 {
1214  Matrix J=GeoJacobian();
1215  Matrix dJ(6,DOF); dJ.zero();
1216  double dqj,dqi,a,b,c;
1217  for (unsigned int i=0; i<DOF; i++) // i: col
1218  {
1219  for (unsigned int j=0; j<=i; j++) // j: row
1220  {
1221  dqj=dq[j];
1222 
1223  a=J(4,j)*J(2,i)-J(5,j)*J(1,i);
1224  b=J(5,j)*J(0,i)-J(3,j)*J(2,i);
1225  c=J(3,j)*J(1,i)-J(4,j)*J(0,i);
1226  dJ(0,i)+=dqj*a;
1227  dJ(1,i)+=dqj*b;
1228  dJ(2,i)+=dqj*c;
1229  dJ(3,i)+=dqj*(J(4,j)*J(5,i)-J(5,j)*J(4,i));
1230  dJ(4,i)+=dqj*(J(5,j)*J(3,i)-J(3,j)*J(5,i));
1231  dJ(5,i)+=dqj*(J(3,j)*J(4,i)-J(4,j)*J(3,i));
1232 
1233  if (i!=j)
1234  {
1235  dqi =dq[i];
1236  dJ(0,j)+=dqi*a;
1237  dJ(1,j)+=dqi*b;
1238  dJ(2,j)+=dqi*c;
1239  }
1240  }
1241  }
1242 
1243  return dJ;
1244 
1245  /* OLD IMPLEMENTATION (SLOWER, BUT CLEARER)
1246  prepareForHessian();
1247  Vector tmp(6,0.0);
1248  for (unsigned int i=0; i<DOF; i++)
1249  {
1250  for (unsigned int j=0; j<DOF; j++)
1251  tmp+=fastHessian_ij(j,i)*dq(j);
1252 
1253  dJ.setCol(i,tmp);
1254  tmp.zero();
1255  }*/
1256 }
1257 
1258 
1259 /************************************************************************/
1260 Matrix iKinChain::DJacobian(const unsigned int lnk, const Vector &dq)
1261 {
1262  Matrix J=GeoJacobian(lnk);
1263  Matrix dJ(6,lnk-1); dJ.zero();
1264  double dqj,dqi,a,b,c;
1265  for (unsigned int i=0; i<lnk; i++) // i: col
1266  {
1267  for (unsigned int j=0; j<=i; j++) // j: row
1268  {
1269  dqj=dq[j];
1270 
1271  a=J(4,j)*J(2,i)-J(5,j)*J(1,i);
1272  b=J(5,j)*J(0,i)-J(3,j)*J(2,i);
1273  c=J(3,j)*J(1,i)-J(4,j)*J(0,i);
1274  dJ(0,i)+=dqj*a;
1275  dJ(1,i)+=dqj*b;
1276  dJ(2,i)+=dqj*c;
1277  dJ(3,i)+=dqj*(J(4,j)*J(5,i)-J(5,j)*J(4,i));
1278  dJ(4,i)+=dqj*(J(5,j)*J(3,i)-J(3,j)*J(5,i));
1279  dJ(5,i)+=dqj*(J(3,j)*J(4,i)-J(4,j)*J(3,i));
1280 
1281  if (i!=j)
1282  {
1283  dqi =dq[i];
1284  dJ(0,j)+=dqi*a;
1285  dJ(1,j)+=dqi*b;
1286  dJ(2,j)+=dqi*c;
1287  }
1288  }
1289  }
1290 
1291  return dJ;
1292 
1293  // OLD IMPLEMENTATION (SLOWER, BUT CLEARER)
1294  /*prepareForHessian(lnk);
1295  Vector tmp(6,0.0);
1296  for (unsigned int i=0; i<lnk; i++)
1297  {
1298  for (unsigned int j=0; j<lnk; j++)
1299  tmp+=fastHessian_ij(lnk,j,i)*dq(j);
1300 
1301  dJ.setCol(i,tmp);
1302  tmp.zero();
1303  }*/
1304 }
1305 
1306 
1307 /************************************************************************/
1309 {
1310  dispose();
1311 }
1312 
1313 
1314 /************************************************************************/
1316 {
1317  allList.clear();
1318  quickList.clear();
1319 }
1320 
1321 
1322 /************************************************************************/
1324 {
1325  allocate("right");
1326 }
1327 
1328 
1329 /************************************************************************/
1330 iKinLimb::iKinLimb(const string &_type)
1331 {
1332  allocate(_type);
1333 }
1334 
1335 
1336 /************************************************************************/
1338 {
1339  clone(limb);
1340 }
1341 
1342 
1343 /************************************************************************/
1344 iKinLimb::iKinLimb(const Property &options)
1345 {
1346  fromLinksProperties(options);
1347 }
1348 
1349 
1350 /************************************************************************/
1352 {
1353  linkList.push_back(pl);
1354  pushLink(*pl);
1355 }
1356 
1357 
1358 /************************************************************************/
1359 void iKinLimb::getMatrixFromProperties(const Property &options, const string &tag,
1360  Matrix &H)
1361 {
1362  if (Bottle *bH=options.find(tag).asList())
1363  {
1364  int i=0;
1365  int j=0;
1366 
1367  H.zero();
1368  for (int cnt=0; (cnt<bH->size()) && (cnt<H.rows()*H.cols()); cnt++)
1369  {
1370  H(i,j)=bH->get(cnt).asDouble();
1371  if (++j>=H.cols())
1372  {
1373  i++;
1374  j=0;
1375  }
1376  }
1377  }
1378 }
1379 
1380 
1381 /************************************************************************/
1382 void iKinLimb::setMatrixToProperties(Property &options, const string &tag,
1383  Matrix &H)
1384 {
1385  Bottle b; Bottle &l=b.addList();
1386  for (int r=0; r<H.rows(); r++)
1387  for (int c=0; c<H.cols(); c++)
1388  l.addDouble(H(r,c));
1389 
1390  options.put(tag,b.get(0));
1391 }
1392 
1393 
1394 /************************************************************************/
1395 bool iKinLimb::fromLinksProperties(const Property &options)
1396 {
1397  dispose();
1398 
1399  type=options.check("type",Value("right")).asString();
1400 
1401  getMatrixFromProperties(options,"H0",H0);
1402  getMatrixFromProperties(options,"HN",HN);
1403 
1404  int numLinks=options.check("numLinks",Value(0)).asInt();
1405  if (numLinks==0)
1406  {
1407  yError("invalid number of links specified!");
1408 
1409  type="right";
1410  H0.eye();
1411  HN.eye();
1412 
1413  return false;
1414  }
1415 
1416  for (int i=0; i<numLinks; i++)
1417  {
1418  ostringstream link;
1419  link<<"link_"<<i;
1420 
1421  Bottle &bLink=options.findGroup(link.str());
1422  if (bLink.isNull())
1423  {
1424  yError("%s is missing!",link.str().c_str());
1425 
1426  type="right";
1427  H0.eye();
1428  dispose();
1429 
1430  return false;
1431  }
1432 
1433  double A=bLink.check("A",Value(0.0)).asDouble();
1434  double D=bLink.check("D",Value(0.0)).asDouble();
1435  double alpha=CTRL_DEG2RAD*bLink.check("alpha",Value(0.0)).asDouble();
1436  double offset=CTRL_DEG2RAD*bLink.check("offset",Value(0.0)).asDouble();
1437  double min=CTRL_DEG2RAD*bLink.check("min",Value(-180.0)).asDouble();
1438  double max=CTRL_DEG2RAD*bLink.check("max",Value(180.0)).asDouble();
1439 
1440  pushLink(new iKinLink(A,D,alpha,offset,min,max));
1441 
1442  if (bLink.check("blocked"))
1443  blockLink(i,CTRL_DEG2RAD*bLink.find("blocked").asDouble());
1444  }
1445 
1446  return true;
1447 }
1448 
1449 
1450 /************************************************************************/
1451 bool iKinLimb::toLinksProperties(Property &options)
1452 {
1453  Bottle links;
1454  for (unsigned int i=0; i<N; i++)
1455  {
1456  Bottle &link=links.addList();
1457  ostringstream tag;
1458  tag<<"link_"<<i;
1459  link.addString(tag.str());
1460 
1461  Bottle &A=link.addList();
1462  A.addString("A");
1463  A.addDouble((*this)[i].getA());
1464 
1465  Bottle &D=link.addList();
1466  D.addString("D");
1467  D.addDouble((*this)[i].getD());
1468 
1469  Bottle &alpha=link.addList();
1470  alpha.addString("alpha");
1471  alpha.addDouble(CTRL_RAD2DEG*(*this)[i].getAlpha());
1472 
1473  Bottle &offset=link.addList();
1474  offset.addString("offset");
1475  offset.addDouble(CTRL_RAD2DEG*(*this)[i].getOffset());
1476 
1477  Bottle &min=link.addList();
1478  min.addString("min");
1479  min.addDouble(CTRL_RAD2DEG*(*this)[i].getMin());
1480 
1481  Bottle &max=link.addList();
1482  max.addString("max");
1483  max.addDouble(CTRL_RAD2DEG*(*this)[i].getMax());
1484 
1485  if ((*this)[i].isBlocked())
1486  {
1487  Bottle &blocked=link.addList();
1488  blocked.addString("blocked");
1489  blocked.addDouble(CTRL_RAD2DEG*(*this)[i].getAng());
1490  }
1491  }
1492 
1493  links.write(options);
1494 
1495  options.put("type",getType());
1496  options.put("numLinks",(int)N);
1497 
1498  setMatrixToProperties(options,"H0",H0);
1499  setMatrixToProperties(options,"HN",HN);
1500 
1501  return true;
1502 }
1503 
1504 
1505 /************************************************************************/
1507 {
1508  dispose();
1509  clone(limb);
1510 
1511  return *this;
1512 }
1513 
1514 
1515 /************************************************************************/
1517 {
1518  dispose();
1519 }
1520 
1521 
1522 /************************************************************************/
1523 void iKinLimb::allocate(const string &_type)
1524 {
1525  type=_type;
1526 }
1527 
1528 
1529 /************************************************************************/
1530 void iKinLimb::clone(const iKinLimb &limb)
1531 {
1532  type=limb.type;
1533  H0=limb.H0;
1534  HN=limb.HN;
1535 
1536  for (unsigned int i=0; i<limb.getN(); i++)
1537  pushLink(new iKinLink(*(limb.linkList[i])));
1538 }
1539 
1540 
1541 /************************************************************************/
1543 {
1544  for (unsigned int i=0; i<linkList.size(); i++)
1545  if (linkList[i]!=NULL)
1546  delete linkList[i];
1547 
1548  linkList.clear();
1550 }
1551 
1552 
1553 /************************************************************************/
1555 {
1556  allocate(getType());
1557 }
1558 
1559 
1560 /************************************************************************/
1561 iCubTorso::iCubTorso(const string &_type) : iKinLimb(_type)
1562 {
1563  allocate(_type);
1564 }
1565 
1566 
1567 /************************************************************************/
1568 void iCubTorso::allocate(const string &_type)
1569 {
1570  if (getType().size()>1)
1571  version=strtod(getType().substr(1).c_str(),NULL);
1572  else
1573  version=1.0;
1574 
1575  Matrix H0(4,4);
1576  H0.zero();
1577  H0(3,3)=1.0;
1578 
1579  if (version<3.0)
1580  {
1581  H0(0,1)=-1.0;
1582  H0(1,2)=-1.0;
1583  H0(2,0)=1.0;
1584  setH0(H0);
1585 
1586  pushLink(new iKinLink( 0.032, 0.0, M_PI/2.0, 0.0, -22.0*CTRL_DEG2RAD, 84.0*CTRL_DEG2RAD));
1587  pushLink(new iKinLink( 0.0, -0.0055, M_PI/2.0, -M_PI/2.0, -39.0*CTRL_DEG2RAD, 39.0*CTRL_DEG2RAD));
1588  pushLink(new iKinLink(0.00231, -0.1933, -M_PI/2.0, -M_PI/2.0, -59.0*CTRL_DEG2RAD, 59.0*CTRL_DEG2RAD));
1589  }
1590  else
1591  {
1592  H0(0,2)=1.0;
1593  H0(1,1)=-1.0;
1594  H0(2,0)=1.0;
1595  setH0(H0);
1596 
1597  pushLink(new iKinLink( 0.0725, 0.0, -M_PI/2.0, 0.0, -20.0*CTRL_DEG2RAD, 20.0*CTRL_DEG2RAD));
1598  pushLink(new iKinLink( 0.0, 0.0, M_PI/2.0, -M_PI/2.0, -15.0*CTRL_DEG2RAD, 40.0*CTRL_DEG2RAD));
1599  pushLink(new iKinLink(-0.00486151, -0.26377, -M_PI/2.0, M_PI, -40.0*CTRL_DEG2RAD, 40.0*CTRL_DEG2RAD));
1600  }
1601 }
1602 
1603 
1604 /************************************************************************/
1605 bool iCubTorso::alignJointsBounds(const deque<IControlLimits*> &lim)
1606 {
1607  if (lim.size()==0)
1608  return false;
1609 
1610  IControlLimits &limTorso=*lim[0];
1611 
1612  unsigned int iTorso;
1613  double min, max;
1614 
1615  for (iTorso=0; iTorso<3; iTorso++)
1616  {
1617  if (!limTorso.getLimits(iTorso,&min,&max))
1618  return false;
1619 
1620  if (version<3.0)
1621  {
1622  (*this)[2-iTorso].setMin(CTRL_DEG2RAD*min);
1623  (*this)[2-iTorso].setMax(CTRL_DEG2RAD*max);
1624  }
1625  else
1626  {
1627  (*this)[iTorso].setMin(CTRL_DEG2RAD*min);
1628  (*this)[iTorso].setMax(CTRL_DEG2RAD*max);
1629  }
1630  }
1631 
1632  return true;
1633 }
1634 
1635 
1636 /************************************************************************/
1638 {
1639  allocate(getType());
1640 }
1641 
1642 
1643 /************************************************************************/
1644 iCubArm::iCubArm(const string &_type) : iKinLimb(_type)
1645 {
1646  allocate(_type);
1647 }
1648 
1649 
1650 /************************************************************************/
1651 void iCubArm::allocate(const string &_type)
1652 {
1653  string arm;
1654  size_t underscore=getType().find('_');
1655  if (underscore!=string::npos)
1656  {
1657  arm=getType().substr(0,underscore);
1658  version=strtod(getType().substr(underscore+2).c_str(),NULL);
1659  }
1660  else
1661  {
1662  arm=getType();
1663  version=1.0;
1664  }
1665 
1666  Matrix H0(4,4);
1667  H0.zero();
1668 
1669  if (version<3.0)
1670  {
1671  H0(0,1)=-1.0;
1672  H0(1,2)=-1.0;
1673  H0(2,0)=1.0;
1674  }
1675  else
1676  {
1677  H0(0,2)=1.0;
1678  H0(1,1)=-1.0;
1679  H0(2,0)=1.0;
1680  }
1681 
1682  H0(3,3)=1.0;
1683  setH0(H0);
1684 
1685  if (arm=="right")
1686  {
1687  if (version<3.0)
1688  {
1689  pushLink(new iKinLink( 0.032, 0.0, M_PI/2.0, 0.0, -22.0*CTRL_DEG2RAD, 84.0*CTRL_DEG2RAD));
1690  pushLink(new iKinLink( 0.0, -0.0055, M_PI/2.0, -M_PI/2.0, -39.0*CTRL_DEG2RAD, 39.0*CTRL_DEG2RAD));
1691  pushLink(new iKinLink(-0.0233647, -0.1433, M_PI/2.0, -105.0*CTRL_DEG2RAD, -59.0*CTRL_DEG2RAD, 59.0*CTRL_DEG2RAD));
1692  pushLink(new iKinLink( 0.0, -0.10774, M_PI/2.0, -M_PI/2.0, -95.5*CTRL_DEG2RAD, 5.0*CTRL_DEG2RAD));
1693  pushLink(new iKinLink( 0.0, 0.0, -M_PI/2.0, -M_PI/2.0, 0.0*CTRL_DEG2RAD, 160.8*CTRL_DEG2RAD));
1694  pushLink(new iKinLink( -0.015, -0.15228, -M_PI/2.0, -105.0*CTRL_DEG2RAD, -37.0*CTRL_DEG2RAD, 100.0*CTRL_DEG2RAD));
1695  pushLink(new iKinLink( 0.015, 0.0, M_PI/2.0, 0.0, 5.5*CTRL_DEG2RAD, 106.0*CTRL_DEG2RAD));
1696  if (version<1.7)
1697  pushLink(new iKinLink( 0.0, -0.1373, M_PI/2.0, -M_PI/2.0, -50.0*CTRL_DEG2RAD, 50.0*CTRL_DEG2RAD));
1698  else
1699  pushLink(new iKinLink( 0.0, -0.1413, M_PI/2.0, -M_PI/2.0, -50.0*CTRL_DEG2RAD, 50.0*CTRL_DEG2RAD));
1700  pushLink(new iKinLink( 0.0, 0.0, M_PI/2.0, M_PI/2.0, -65.0*CTRL_DEG2RAD, 10.0*CTRL_DEG2RAD));
1701  if (version<2.0)
1702  pushLink(new iKinLink( 0.0625, 0.016, 0.0, M_PI, -25.0*CTRL_DEG2RAD, 25.0*CTRL_DEG2RAD));
1703  else
1704  pushLink(new iKinLink( 0.0625, 0.02598, 0.0, M_PI, -25.0*CTRL_DEG2RAD, 25.0*CTRL_DEG2RAD));
1705  }
1706  else
1707  {
1708  pushLink(new iKinLink( 0.0725, 0.0, -M_PI/2.0, 0.0, -20.0*CTRL_DEG2RAD, 20.0*CTRL_DEG2RAD));
1709  pushLink(new iKinLink( 0.0, 0.0, M_PI/2.0, -M_PI/2.0, -15.0*CTRL_DEG2RAD, 40.0*CTRL_DEG2RAD));
1710  pushLink(new iKinLink(-0.0304112, -0.161133, 75.489181*CTRL_DEG2RAD, 165.0*CTRL_DEG2RAD, -40.0*CTRL_DEG2RAD, 40.0*CTRL_DEG2RAD));
1711  pushLink(new iKinLink( 0.0, -0.163978, M_PI/2.0, -M_PI/2.0, -90.0*CTRL_DEG2RAD, 2.0*CTRL_DEG2RAD));
1712  pushLink(new iKinLink( 0.0, 0.0, -M_PI/2.0, -104.5*CTRL_DEG2RAD, -5.0*CTRL_DEG2RAD, 140.0*CTRL_DEG2RAD));
1713  pushLink(new iKinLink( -0.01065, -0.2005, -M_PI/2.0, -105*CTRL_DEG2RAD, -45.0*CTRL_DEG2RAD, 80.0*CTRL_DEG2RAD));
1714  pushLink(new iKinLink( 0.0146593, 0.00308819, M_PI/2.0, 0.0, -5.0*CTRL_DEG2RAD, 110.0*CTRL_DEG2RAD));
1715  pushLink(new iKinLink(-0.0020075, -0.1628, M_PI/2.0, -M_PI/2.0, -60.0*CTRL_DEG2RAD, 60.0*CTRL_DEG2RAD));
1716  pushLink(new iKinLink( 0.0, 0.0, M_PI/2.0, M_PI/2.0, -70.0*CTRL_DEG2RAD, 25.0*CTRL_DEG2RAD));
1717  pushLink(new iKinLink( 0.0625, 0.016, 0.0, M_PI, -15.0*CTRL_DEG2RAD, 35.0*CTRL_DEG2RAD));
1718  }
1719  }
1720  else
1721  {
1722  if (arm!="left")
1723  type.replace(0,underscore,"left");
1724 
1725  if (version<3.0)
1726  {
1727  pushLink(new iKinLink( 0.032, 0.0, M_PI/2.0, 0.0, -22.0*CTRL_DEG2RAD, 84.0*CTRL_DEG2RAD));
1728  pushLink(new iKinLink( 0.0, -0.0055, M_PI/2.0, -M_PI/2.0, -39.0*CTRL_DEG2RAD, 39.0*CTRL_DEG2RAD));
1729  pushLink(new iKinLink( 0.0233647, -0.1433, -M_PI/2.0, 105.0*CTRL_DEG2RAD, -59.0*CTRL_DEG2RAD, 59.0*CTRL_DEG2RAD));
1730  pushLink(new iKinLink( 0.0, 0.10774, -M_PI/2.0, M_PI/2.0, -95.5*CTRL_DEG2RAD, 5.0*CTRL_DEG2RAD));
1731  pushLink(new iKinLink( 0.0, 0.0, M_PI/2.0, -M_PI/2.0, 0.0*CTRL_DEG2RAD, 160.8*CTRL_DEG2RAD));
1732  pushLink(new iKinLink( 0.015, 0.15228, -M_PI/2.0, 75.0*CTRL_DEG2RAD, -37.0*CTRL_DEG2RAD, 100.0*CTRL_DEG2RAD));
1733  pushLink(new iKinLink( -0.015, 0.0, M_PI/2.0, 0.0, 5.5*CTRL_DEG2RAD, 106.0*CTRL_DEG2RAD));
1734  if (version<1.7)
1735  pushLink(new iKinLink( 0.0, 0.1373, M_PI/2.0, -M_PI/2.0, -50.0*CTRL_DEG2RAD, 50.0*CTRL_DEG2RAD));
1736  else
1737  pushLink(new iKinLink( 0.0, 0.1413, M_PI/2.0, -M_PI/2.0, -50.0*CTRL_DEG2RAD, 50.0*CTRL_DEG2RAD));
1738  pushLink(new iKinLink( 0.0, 0.0, M_PI/2.0, M_PI/2.0, -65.0*CTRL_DEG2RAD, 10.0*CTRL_DEG2RAD));
1739  if (version<2.0)
1740  pushLink(new iKinLink( 0.0625, -0.016, 0.0, 0.0, -25.0*CTRL_DEG2RAD, 25.0*CTRL_DEG2RAD));
1741  else
1742  pushLink(new iKinLink( 0.0625, -0.02598, 0.0, 0.0, -25.0*CTRL_DEG2RAD, 25.0*CTRL_DEG2RAD));
1743  }
1744  else
1745  {
1746  pushLink(new iKinLink( 0.0725, 0.0, -M_PI/2.0, 0.0, -20.0*CTRL_DEG2RAD, 20.0*CTRL_DEG2RAD));
1747  pushLink(new iKinLink( 0.0, 0.0, M_PI/2.0, -M_PI/2.0, -15.0*CTRL_DEG2RAD, 40.0*CTRL_DEG2RAD));
1748  pushLink(new iKinLink( 0.0304112, -0.161133, -104.510819*CTRL_DEG2RAD, 15.0*CTRL_DEG2RAD, -40.0*CTRL_DEG2RAD, 40.0*CTRL_DEG2RAD));
1749  pushLink(new iKinLink( 0.0, 0.163978, M_PI/2.0, -M_PI/2.0, -90.0*CTRL_DEG2RAD, 2.0*CTRL_DEG2RAD));
1750  pushLink(new iKinLink( 0.0, 0.0, M_PI/2.0, 75.5*CTRL_DEG2RAD, -5.0*CTRL_DEG2RAD, 140.0*CTRL_DEG2RAD));
1751  pushLink(new iKinLink( 0.01065, 0.2005, -M_PI/2.0, 75.0*CTRL_DEG2RAD, -45.0*CTRL_DEG2RAD, 80.0*CTRL_DEG2RAD));
1752  pushLink(new iKinLink(-0.0146593, -0.00308819, M_PI/2.0, 0.0, -5.0*CTRL_DEG2RAD, 110.0*CTRL_DEG2RAD));
1753  pushLink(new iKinLink( 0.0020075, 0.1628, M_PI/2.0, -M_PI/2.0, -60.0*CTRL_DEG2RAD, 60.0*CTRL_DEG2RAD));
1754  pushLink(new iKinLink( 0.0, 0.0, M_PI/2.0, M_PI/2.0, -70.0*CTRL_DEG2RAD, 25.0*CTRL_DEG2RAD));
1755  pushLink(new iKinLink( 0.0625, -0.016, 0.0, 0.0, -15.0*CTRL_DEG2RAD, 35.0*CTRL_DEG2RAD));
1756  }
1757  }
1758 
1759  blockLink(0,0.0);
1760  blockLink(1,0.0);
1761  blockLink(2,0.0);
1762 }
1763 
1764 
1765 /************************************************************************/
1766 bool iCubArm::alignJointsBounds(const deque<IControlLimits*> &lim)
1767 {
1768  if (lim.size()<2)
1769  return false;
1770 
1771  IControlLimits &limTorso=*lim[0];
1772  IControlLimits &limArm =*lim[1];
1773 
1774  unsigned int iTorso;
1775  unsigned int iArm;
1776  double min, max;
1777 
1778  for (iTorso=0; iTorso<3; iTorso++)
1779  {
1780  if (!limTorso.getLimits(iTorso,&min,&max))
1781  return false;
1782 
1783  if (version<3.0)
1784  {
1785  (*this)[2-iTorso].setMin(CTRL_DEG2RAD*min);
1786  (*this)[2-iTorso].setMax(CTRL_DEG2RAD*max);
1787  }
1788  else
1789  {
1790  (*this)[iTorso].setMin(CTRL_DEG2RAD*min);
1791  (*this)[iTorso].setMax(CTRL_DEG2RAD*max);
1792  }
1793  }
1794 
1795  for (iArm=0; iArm<getN()-iTorso; iArm++)
1796  {
1797  if (!limArm.getLimits(iArm,&min,&max))
1798  return false;
1799 
1800  (*this)[iTorso+iArm].setMin(CTRL_DEG2RAD*min);
1801  (*this)[iTorso+iArm].setMax(CTRL_DEG2RAD*max);
1802  }
1803 
1804  return true;
1805 }
1806 
1807 
1808 /************************************************************************/
1810 {
1811  allocate(getType());
1812 }
1813 
1814 
1815 /************************************************************************/
1816 iCubFinger::iCubFinger(const string &_type) : iKinLimb(_type)
1817 {
1818  allocate(_type);
1819 }
1820 
1821 
1822 /************************************************************************/
1824 {
1825  clone(finger);
1826 }
1827 
1828 
1829 /************************************************************************/
1830 void iCubFinger::clone(const iCubFinger &finger)
1831 {
1832  this->hand=finger.hand;
1833  this->finger=finger.finger;
1834  this->version=finger.version;
1835 }
1836 
1837 
1838 /************************************************************************/
1840 {
1841  dispose();
1843  clone(finger);
1844 
1845  return *this;
1846 }
1847 
1848 
1849 /************************************************************************/
1850 void iCubFinger::allocate(const string &_type)
1851 {
1852  size_t underscore=getType().find('_');
1853  if (underscore!=string::npos)
1854  {
1855  hand=getType().substr(0,underscore);
1856  finger=getType().substr(underscore+1,getType().length()-underscore-1);
1857 
1858  underscore=finger.find('_');
1859  if (underscore!=string::npos)
1860  {
1861  version=finger.substr(underscore+1,finger.length()-underscore-1);
1862  finger=finger.substr(0,underscore);
1863  }
1864  else
1865  version="na";
1866  }
1867  else
1868  {
1869  hand="right";
1870  finger="index";
1871  version="na";
1872  }
1873 
1874  type=hand+"_"+finger+"_"+version;
1875 
1876  // reinforce hand info
1877  if (hand!="left")
1878  hand="right";
1879 
1880  Matrix H0(4,4);
1881  if (finger=="thumb")
1882  {
1883  if (version=="a")
1884  {
1885  H0(0,0)=0.121132; H0(0,1)=0.043736; H0(0,2)=-0.991672; H0(0,3)=-0.025391770;
1886  H0(1,0)=-0.958978; H0(1,1)=0.263104; H0(1,2)=-0.105535; H0(1,3)=-0.011783901;
1887  H0(2,0)=0.256297; H0(2,1)=0.963776; H0(2,2)=0.073812; H0(2,3)=-0.0017018;
1888  H0(3,0)=0.0; H0(3,1)=0.0; H0(3,2)=0.0; H0(3,3)=1.0;
1889  }
1890  else
1891  {
1892  version="b";
1893  H0(0,0)=0.478469; H0(0,1)=0.063689; H0(0,2)=-0.875792; H0(0,3)=-0.024029759;
1894  H0(1,0)=-0.878095; H0(1,1)=0.039246; H0(1,2)=-0.476873; H0(1,3)=-0.01193433;
1895  H0(2,0)=0.004; H0(2,1)=0.997198; H0(2,2)=0.074703; H0(2,3)=-0.00168926;
1896  H0(3,0)=0.0; H0(3,1)=0.0; H0(3,2)=0.0; H0(3,3)=1.0;
1897  }
1898 
1899  if (hand=="left")
1900  {
1901  H0(2,1)=-H0(2,1);
1902  H0(0,2)=-H0(0,2);
1903  H0(1,2)=-H0(1,2);
1904  H0(2,3)=-H0(2,3);
1905 
1906  pushLink(new iKinLink( 0.0, 0.0, M_PI/2.0, 0.0, 10.0*CTRL_DEG2RAD, 90.0*CTRL_DEG2RAD));
1907  pushLink(new iKinLink(0.0210, -0.0056, 0.0, 0.0, 0.0*CTRL_DEG2RAD, 0.0*CTRL_DEG2RAD));
1908  }
1909  else
1910  {
1911  pushLink(new iKinLink( 0.0, 0.0, -M_PI/2.0, 0.0, 10.0*CTRL_DEG2RAD, 90.0*CTRL_DEG2RAD));
1912  pushLink(new iKinLink(0.0210, 0.0056, 0.0, 0.0, 0.0*CTRL_DEG2RAD, 0.0*CTRL_DEG2RAD));
1913  }
1914 
1915  pushLink(new iKinLink(0.0260, 0.0, 0.0, 0.0, 0.0*CTRL_DEG2RAD, 90.0*CTRL_DEG2RAD));
1916  pushLink(new iKinLink(0.0220, 0.0, 0.0, 0.0, 0.0*CTRL_DEG2RAD, 90.0*CTRL_DEG2RAD));
1917  pushLink(new iKinLink(0.0168, 0.0, -M_PI/2.0, 0.0, 0.0*CTRL_DEG2RAD, 90.0*CTRL_DEG2RAD));
1918 
1919  // this is a dummy link
1920  blockLink(1,0.0);
1921  }
1922  else if (finger=="index")
1923  {
1924  H0(0,0)=0.898138; H0(0,1)=0.439714; H0(0,2)=0.0; H0(0,3)=0.00245549;
1925  H0(1,0)=-0.43804; H0(1,1)=0.89472; H0(1,2)=0.087156; H0(1,3)=-0.025320433;
1926  H0(2,0)=0.038324; H0(2,1)=-0.078278; H0(2,2)=0.996195; H0(2,3)=-0.010973325;
1927  H0(3,0)=0.0; H0(3,1)=0.0; H0(3,2)=0.0; H0(3,3)=1.0;
1928 
1929  if (hand=="left")
1930  {
1931  H0(2,0)=-H0(2,0);
1932  H0(2,1)=-H0(2,1);
1933  H0(1,2)=-H0(1,2);
1934  H0(2,3)=-H0(2,3);
1935 
1936  pushLink(new iKinLink(0.0148, 0.0, -M_PI/2.0, 0.0, 0.0, 20.0*CTRL_DEG2RAD));
1937  }
1938  else
1939  pushLink(new iKinLink(0.0148, 0.0, M_PI/2.0, 0.0, 0.0, 20.0*CTRL_DEG2RAD));
1940 
1941  pushLink(new iKinLink(0.0259, 0.0, 0.0, 0.0, 0.0, 90.0*CTRL_DEG2RAD));
1942  pushLink(new iKinLink(0.0220, 0.0, 0.0, 0.0, 0.0, 90.0*CTRL_DEG2RAD));
1943  pushLink(new iKinLink(0.0168, 0.0, -M_PI/2.0, 0.0, 0.0, 90.0*CTRL_DEG2RAD));
1944  }
1945  else if (finger=="middle")
1946  {
1947  H0(0,0)=1.0; H0(0,1)=0.0; H0(0,2)=0.0; H0(0,3)=0.0178;
1948  H0(1,0)=0.0; H0(1,1)=0.0; H0(1,2)=-1.0; H0(1,3)=-0.00830233;
1949  H0(2,0)=0.0; H0(2,1)=1.0; H0(2,2)=0.0; H0(2,3)=-0.0118;
1950  H0(3,0)=0.0; H0(3,1)=0.0; H0(3,2)=0.0; H0(3,3)=1.0;
1951 
1952  if (hand=="left")
1953  {
1954  H0(2,1)=-H0(2,1);
1955  H0(1,2)=-H0(1,2);
1956  H0(2,3)=-H0(2,3);
1957  }
1958 
1959  pushLink(new iKinLink(0.0285, 0.0, 0.0, 0.0, 0.0, 90.0*CTRL_DEG2RAD));
1960  pushLink(new iKinLink(0.0240, 0.0, 0.0, 0.0, 0.0, 90.0*CTRL_DEG2RAD));
1961  pushLink(new iKinLink(0.0168, 0.0, -M_PI/2.0, 0.0, 0.0, 90.0*CTRL_DEG2RAD));
1962  }
1963  else if (finger=="ring")
1964  {
1965  H0(0,0)=0.9998; H0(0,1)=0.0192; H0(0,2)=0.0; H0(0,3)=0.001569230;
1966  H0(1,0)=0.0191; H0(1,1)=-0.9960; H0(1,2)=0.0872; H0(1,3)=0.007158757;
1967  H0(2,0)=0.0017; H0(2,1)=-0.0871; H0(2,2)=-0.9962; H0(2,3)=-0.011458935;
1968  H0(3,0)=0.0; H0(3,1)=0.0; H0(3,2)=0.0; H0(3,3)=1.0;
1969 
1970  if (hand=="left")
1971  {
1972  H0(2,0)=-H0(2,0);
1973  H0(2,1)=-H0(2,1);
1974  H0(1,2)=-H0(1,2);
1975  H0(2,3)=-H0(2,3);
1976 
1977  pushLink(new iKinLink(0.0148, 0.0, M_PI/2.0, -20.0*CTRL_DEG2RAD, 0.0, 20.0*CTRL_DEG2RAD));
1978  }
1979  else
1980  pushLink(new iKinLink(0.0148, 0.0, -M_PI/2.0, -20.0*CTRL_DEG2RAD, 0.0, 20.0*CTRL_DEG2RAD));
1981 
1982  pushLink(new iKinLink(0.0259, 0.0, 0.0, 0.0, 0.0, 90.0*CTRL_DEG2RAD));
1983  pushLink(new iKinLink(0.0220, 0.0, 0.0, 0.0, 0.0, 90.0*CTRL_DEG2RAD));
1984  pushLink(new iKinLink(0.0168, 0.0, -M_PI/2.0, 0.0, 0.0, 90.0*CTRL_DEG2RAD));
1985  }
1986  else if (finger=="little")
1987  {
1988  H0(0,0)=0.9998; H0(0,1)=0.0192; H0(0,2)=0.0; H0(0,3)=-0.00042147;
1989  H0(1,0)=0.0191; H0(1,1)=-0.9960; H0(1,2)=0.0872; H0(1,3)=0.0232755;
1990  H0(2,0)=0.0017; H0(2,1)=-0.0871; H0(2,2)=-0.9962; H0(2,3)=-0.00956329;
1991  H0(3,0)=0.0; H0(3,1)=0.0; H0(3,2)=0.0; H0(3,3)=1.0;
1992 
1993  if (hand=="left")
1994  {
1995  H0(2,0)=-H0(2,0);
1996  H0(2,1)=-H0(2,1);
1997  H0(1,2)=-H0(1,2);
1998  H0(2,3)=-H0(2,3);
1999 
2000  pushLink(new iKinLink(0.0148, 0.0, M_PI/2.0, -20.0*CTRL_DEG2RAD, 0.0, 20.0*CTRL_DEG2RAD));
2001  }
2002  else
2003  pushLink(new iKinLink(0.0148, 0.0, -M_PI/2.0, -20.0*CTRL_DEG2RAD, 0.0, 20.0*CTRL_DEG2RAD));
2004 
2005  pushLink(new iKinLink(0.0219, 0.0, 0.0, 0.0, 0.0, 90.0*CTRL_DEG2RAD));
2006  pushLink(new iKinLink(0.0190, 0.0, 0.0, 0.0, 0.0, 90.0*CTRL_DEG2RAD));
2007  pushLink(new iKinLink(0.0168, 0.0, -M_PI/2.0, 0.0, 0.0, 90.0*CTRL_DEG2RAD));
2008  }
2009 
2010  if ((finger=="index") || (finger=="ring") || (finger=="little"))
2011  {
2012  // evaluate maximum fingers abduction using the default limits
2013  fingers_abduction_max = (*this)[0].getMax()*3.0*CTRL_RAD2DEG;
2014  }
2015 
2016  setH0(H0);
2017 }
2018 
2019 
2020 /************************************************************************/
2021 bool iCubFinger::alignJointsBounds(const deque<IControlLimits*> &lim)
2022 {
2023  if (lim.size()<1)
2024  return false;
2025 
2026  IControlLimits &limFinger=*lim[0];
2027  double min, max;
2028 
2029  if (finger=="thumb")
2030  {
2031  if (!limFinger.getLimits(8,&min,&max))
2032  return false;
2033 
2034  (*this)[0].setMin(CTRL_DEG2RAD*min);
2035  (*this)[0].setMax(CTRL_DEG2RAD*max);
2036 
2037  if (!limFinger.getLimits(9,&min,&max))
2038  return false;
2039 
2040  (*this)[2].setMin(CTRL_DEG2RAD*min);
2041  (*this)[2].setMax(CTRL_DEG2RAD*max);
2042 
2043  if (!limFinger.getLimits(10,&min,&max))
2044  return false;
2045 
2046  (*this)[3].setMin(CTRL_DEG2RAD*min);
2047  (*this)[3].setMax(CTRL_DEG2RAD*max/2.0);
2048  (*this)[4].setMin(CTRL_DEG2RAD*min);
2049  (*this)[4].setMax(CTRL_DEG2RAD*max/2.0);
2050  }
2051  else if (finger=="index")
2052  {
2053  if (!limFinger.getLimits(7,&min,&max))
2054  return false;
2055 
2057 
2058  (*this)[0].setMin(0.0);
2059  (*this)[0].setMax(CTRL_DEG2RAD*(max-min)/3.0);
2060 
2061  if (!limFinger.getLimits(11,&min,&max))
2062  return false;
2063 
2064  (*this)[1].setMin(CTRL_DEG2RAD*min);
2065  (*this)[1].setMax(CTRL_DEG2RAD*max);
2066 
2067  if (!limFinger.getLimits(12,&min,&max))
2068  return false;
2069 
2070  (*this)[2].setMin(CTRL_DEG2RAD*min);
2071  (*this)[2].setMax(CTRL_DEG2RAD*max/2.0);
2072  (*this)[3].setMin(CTRL_DEG2RAD*min);
2073  (*this)[3].setMax(CTRL_DEG2RAD*max/2.0);
2074  }
2075  else if (finger=="middle")
2076  {
2077  if (!limFinger.getLimits(13,&min,&max))
2078  return false;
2079 
2080  (*this)[0].setMin(CTRL_DEG2RAD*min);
2081  (*this)[0].setMax(CTRL_DEG2RAD*max);
2082 
2083  if (!limFinger.getLimits(14,&min,&max))
2084  return false;
2085 
2086  (*this)[1].setMin(CTRL_DEG2RAD*min);
2087  (*this)[1].setMax(CTRL_DEG2RAD*max/2.0);
2088  (*this)[2].setMin(CTRL_DEG2RAD*min);
2089  (*this)[2].setMax(CTRL_DEG2RAD*max/2.0);
2090  }
2091  else if ((finger=="ring") || (finger=="little"))
2092  {
2093  if (!limFinger.getLimits(7,&min,&max))
2094  return false;
2095 
2097 
2098  (*this)[0].setMin(0.0);
2099  (*this)[0].setMax(CTRL_DEG2RAD*(max-min)/3.0);
2100 
2101  if (!limFinger.getLimits(15,&min,&max))
2102  return false;
2103 
2104  (*this)[1].setMin(CTRL_DEG2RAD*min);
2105  (*this)[1].setMax(CTRL_DEG2RAD*max/3.0);
2106  (*this)[2].setMin(CTRL_DEG2RAD*min);
2107  (*this)[2].setMax(CTRL_DEG2RAD*max/3.0);
2108  (*this)[3].setMin(CTRL_DEG2RAD*min);
2109  (*this)[3].setMax(CTRL_DEG2RAD*max/3.0);
2110  }
2111 
2112  return true;
2113 }
2114 
2115 
2116 /************************************************************************/
2117 bool iCubFinger::getChainJoints(const Vector &motorEncoders,
2118  Vector &chainJoints)
2119 {
2120  if ((motorEncoders.length()!=9) && (motorEncoders.length()!=16))
2121  return false;
2122 
2123  int offs=(motorEncoders.length()==16?7:0);
2124 
2125  if (finger=="thumb")
2126  {
2127  chainJoints.resize(4);
2128  chainJoints[0]=motorEncoders[offs+1];
2129  chainJoints[1]=motorEncoders[offs+2];
2130  chainJoints[2]=motorEncoders[offs+3]/2.0;
2131  chainJoints[3]=chainJoints[2];
2132  }
2133  else if (finger=="index")
2134  {
2135  chainJoints.resize(4);
2136  chainJoints[0]=(fingers_abduction_max-motorEncoders[offs+0])/3.0;
2137  chainJoints[1]=motorEncoders[offs+4];
2138  chainJoints[2]=motorEncoders[offs+5]/2.0;
2139  chainJoints[3]=chainJoints[2];
2140  }
2141  else if (finger=="middle")
2142  {
2143  chainJoints.resize(3);
2144  chainJoints[0]=motorEncoders[offs+6];
2145  chainJoints[1]=motorEncoders[offs+7]/2.0;
2146  chainJoints[2]=chainJoints[1];
2147  }
2148  else if ((finger=="ring") || (finger=="little"))
2149  {
2150  chainJoints.resize(4);
2151  chainJoints[0]=(fingers_abduction_max-motorEncoders[offs+0])/3.0;
2152  chainJoints[1]=motorEncoders[offs+8]/3.0;
2153  chainJoints[3]=chainJoints[2]=chainJoints[1];
2154  }
2155  else
2156  return false;
2157 
2158  return true;
2159 }
2160 
2161 
2162 /************************************************************************/
2163 bool iCubFinger::getChainJoints(const Vector &motorEncoders,
2164  const Vector &jointEncoders,
2165  Vector &chainJoints,
2166  const Matrix &jointEncodersBounds)
2167 {
2168  if (((motorEncoders.length()!=9) && (motorEncoders.length()!=16)) ||
2169  (jointEncoders.length()<15) || (jointEncodersBounds.cols()<2))
2170  return false;
2171 
2172  int offs=(motorEncoders.length()==16?7:0);
2173 
2174  Matrix bounds=jointEncodersBounds;
2175  if (bounds.rows()!=jointEncoders.length())
2176  {
2177  bounds=zeros((unsigned int)jointEncoders.length(),2);
2178  for (size_t r=0; r<jointEncoders.length(); r++)
2179  bounds(r,0)=255.0;
2180  }
2181 
2182  if (finger=="thumb")
2183  {
2184  chainJoints.resize(4);
2185  chainJoints[0]=motorEncoders[offs+1];
2186  for (unsigned int i=1; i<chainJoints.length(); i++)
2187  {
2188  double c=0.0;
2189  double span=bounds(i-1,1)-bounds(i-1,0);
2190  if (span>0.0)
2191  c=std::min(1.0,std::max(0.0,(jointEncoders[i-1]-bounds(i-1,0))/span));
2192  else if (span<0.0)
2193  c=1.0-std::min(1.0,std::max(0.0,(bounds(i-1,1)-jointEncoders[i-1])/span));
2194  chainJoints[i]=CTRL_RAD2DEG*(c*((*this)(i).getMax()-(*this)(i).getMin())+(*this)(i).getMin());
2195  }
2196  }
2197  else if (finger=="index")
2198  {
2199  chainJoints.resize(4);
2200  chainJoints[0]=(fingers_abduction_max-motorEncoders[offs+0])/3.0;
2201  for (unsigned int i=1; i<chainJoints.length(); i++)
2202  {
2203  double c=0.0;
2204  double span=bounds(i+2,1)-bounds(i+2,0);
2205  if (span>0.0)
2206  c=std::min(1.0,std::max(0.0,(jointEncoders[i+2]-bounds(i+2,0))/span));
2207  else if (span<0.0)
2208  c=1.0-std::min(1.0,std::max(0.0,(bounds(i+2,1)-jointEncoders[i+2])/span));
2209  chainJoints[i]=CTRL_RAD2DEG*(c*((*this)(i).getMax()-(*this)(i).getMin())+(*this)(i).getMin());
2210  }
2211  }
2212  else if (finger=="middle")
2213  {
2214  chainJoints.resize(3);
2215  for (unsigned int i=0; i<chainJoints.length(); i++)
2216  {
2217  double c=0.0;
2218  double span=bounds(i+6,1)-bounds(i+6,0);
2219  if (span>0.0)
2220  c=std::min(1.0,std::max(0.0,(jointEncoders[i+6]-bounds(i+6,0))/span));
2221  else if (span<0.0)
2222  c=1.0-std::min(1.0,std::max(0.0,(bounds(i+6,1)-jointEncoders[i+6])/span));
2223  chainJoints[i]=CTRL_RAD2DEG*(c*((*this)(i).getMax()-(*this)(i).getMin())+(*this)(i).getMin());
2224  }
2225  }
2226  else if (finger=="ring")
2227  {
2228  chainJoints.resize(4);
2229  chainJoints[0]=(fingers_abduction_max-motorEncoders[offs+0])/3.0;
2230  for (unsigned int i=1; i<chainJoints.length(); i++)
2231  {
2232  double c=0.0;
2233  double span=bounds(i+8,1)-bounds(i+8,0);
2234  if (span>0.0)
2235  c=std::min(1.0,std::max(0.0,(jointEncoders[i+8]-bounds(i+8,0))/span));
2236  else if (span<0.0)
2237  c=1.0-std::min(1.0,std::max(0.0,(bounds(i+8,1)-jointEncoders[i+8])/span));
2238  chainJoints[i]=CTRL_RAD2DEG*(c*((*this)(i).getMax()-(*this)(i).getMin())+(*this)(i).getMin());
2239  }
2240  }
2241  else if (finger=="little")
2242  {
2243  chainJoints.resize(4);
2244  chainJoints[0]=(fingers_abduction_max-motorEncoders[offs+0])/3.0;
2245  for (unsigned int i=1; i<chainJoints.length(); i++)
2246  {
2247  double c=0.0;
2248  double span=bounds(i+11,1)-bounds(i+11,0);
2249  if (span>0.0)
2250  c=std::min(1.0,std::max(0.0,(jointEncoders[i+11]-bounds(i+11,0))/span));
2251  else if (span<0.0)
2252  c=1.0-std::min(1.0,std::max(0.0,(bounds(i+11,1)-jointEncoders[i+11])/span));
2253  chainJoints[i]=CTRL_RAD2DEG*(c*((*this)(i).getMax()-(*this)(i).getMin())+(*this)(i).getMin());
2254  }
2255  }
2256  else
2257  return false;
2258 
2259  return true;
2260 }
2261 
2262 
2263 /************************************************************************/
2265 {
2266  allocate(getType());
2267 }
2268 
2269 
2270 /************************************************************************/
2271 iCubLeg::iCubLeg(const string &_type) : iKinLimb(_type)
2272 {
2273  allocate(_type);
2274 }
2275 
2276 
2277 /************************************************************************/
2278 void iCubLeg::allocate(const string &_type)
2279 {
2280  string leg;
2281  size_t underscore=getType().find('_');
2282  if (underscore!=string::npos)
2283  {
2284  leg=getType().substr(0,underscore);
2285  version=strtod(getType().substr(underscore+2).c_str(),NULL);
2286  }
2287  else
2288  {
2289  leg=getType();
2290  version=1.0;
2291  }
2292 
2293  Matrix H0(4,4);
2294  H0.zero();
2295  H0(0,0)=1.0;
2296  H0(1,2)=1.0;
2297  H0(2,1)=-1.0;
2298  H0(2,3)=-0.1199;
2299  H0(3,3)=1.0;
2300 
2301  if (leg=="right")
2302  {
2303  if (version<2.0)
2304  {
2305  H0(1,3)=0.0681;
2306 
2307  pushLink(new iKinLink( 0.0, 0.0, M_PI/2.0, M_PI/2.0, -44.0*CTRL_DEG2RAD, 132.0*CTRL_DEG2RAD));
2308  pushLink(new iKinLink( 0.0, 0.0, M_PI/2.0, M_PI/2.0, -119.0*CTRL_DEG2RAD, 17.0*CTRL_DEG2RAD));
2309  pushLink(new iKinLink( 0.0, 0.2236, -M_PI/2.0, -M_PI/2.0, -79.0*CTRL_DEG2RAD, 79.0*CTRL_DEG2RAD));
2310  pushLink(new iKinLink(-0.213, 0.0, M_PI, M_PI/2.0, -125.0*CTRL_DEG2RAD, 0.0*CTRL_DEG2RAD));
2311  pushLink(new iKinLink( 0.0, 0.0, M_PI/2.0, 0.0, -42.0*CTRL_DEG2RAD, 21.0*CTRL_DEG2RAD));
2312  pushLink(new iKinLink(-0.041, 0.0, M_PI, 0.0, -24.0*CTRL_DEG2RAD, 24.0*CTRL_DEG2RAD));
2313  }
2314  else if (version<3.0)
2315  {
2316  H0(1,3)=0.0681;
2317 
2318  pushLink(new iKinLink( 0.0, 0.0, M_PI/2.0, M_PI/2.0, -44.0*CTRL_DEG2RAD, 132.0*CTRL_DEG2RAD));
2319  pushLink(new iKinLink( 0.0, 0.0, M_PI/2.0, M_PI/2.0, -119.0*CTRL_DEG2RAD, 17.0*CTRL_DEG2RAD));
2320  pushLink(new iKinLink(-0.0009175, 0.234545, -M_PI/2.0, -M_PI/2.0, -79.0*CTRL_DEG2RAD, 79.0*CTRL_DEG2RAD));
2321  pushLink(new iKinLink( -0.2005, 0.0, M_PI, M_PI/2.0, -125.0*CTRL_DEG2RAD, 0.0*CTRL_DEG2RAD));
2322  pushLink(new iKinLink( 0.0, 0.0, M_PI/2.0, 0.0, -42.0*CTRL_DEG2RAD, 21.0*CTRL_DEG2RAD));
2323  pushLink(new iKinLink( -0.06805, 0.0035, M_PI, 0.0, -24.0*CTRL_DEG2RAD, 24.0*CTRL_DEG2RAD));
2324  }
2325  else // version>=3.0
2326  {
2327  H0.zero();
2328  H0(0,0)=1.0;
2329  H0(1,2)=1.0;
2330  H0(2,1)=-1.0;
2331  H0(0,3)=-0.0216;
2332  H0(1,3)=0.07365;
2333  H0(2,3)=-0.0605;
2334  H0(3,3)=1.0;
2335 
2336  pushLink(new iKinLink( 0.009, 0.0, M_PI/2.0, M_PI/2.0, -40.0*CTRL_DEG2RAD, 125.0*CTRL_DEG2RAD));
2337  pushLink(new iKinLink( -0.015, 0.0106, M_PI/2.0, M_PI/2.0, 5.0*CTRL_DEG2RAD, 111.0*CTRL_DEG2RAD));
2338  pushLink(new iKinLink( 0.015, 0.2668, -M_PI/2.0, -M_PI/2.0, -62.5*CTRL_DEG2RAD, 62.5*CTRL_DEG2RAD));
2339  pushLink(new iKinLink(-0.255988, 0.0062, M_PI, M_PI/2.0, -100.0*CTRL_DEG2RAD, 0.0*CTRL_DEG2RAD));
2340  pushLink(new iKinLink( -0.035, 0.0, M_PI/2.0, 0.0, -40.0*CTRL_DEG2RAD, 40.0*CTRL_DEG2RAD));
2341  pushLink(new iKinLink( -0.0532, 0.0, M_PI, 0.0, -20.0*CTRL_DEG2RAD, 20.0*CTRL_DEG2RAD));
2342  }
2343  }
2344  else // left
2345  {
2346  if (version<2.0)
2347  {
2348  H0(1,3)=-0.0681;
2349 
2350  pushLink(new iKinLink( 0.0, 0.0, -M_PI/2.0, M_PI/2.0, -44.0*CTRL_DEG2RAD, 132.0*CTRL_DEG2RAD));
2351  pushLink(new iKinLink( 0.0, 0.0, -M_PI/2.0, M_PI/2.0, -119.0*CTRL_DEG2RAD, 17.0*CTRL_DEG2RAD));
2352  pushLink(new iKinLink( 0.0, -0.2236, M_PI/2.0, -M_PI/2.0, -79.0*CTRL_DEG2RAD, 79.0*CTRL_DEG2RAD));
2353  pushLink(new iKinLink(-0.213, 0.0, M_PI, M_PI/2.0, -125.0*CTRL_DEG2RAD, 0.0*CTRL_DEG2RAD));
2354  pushLink(new iKinLink( 0.0, 0.0, -M_PI/2.0, 0.0, -42.0*CTRL_DEG2RAD, 21.0*CTRL_DEG2RAD));
2355  pushLink(new iKinLink(-0.041, 0.0, 0.0, 0.0, -24.0*CTRL_DEG2RAD, 24.0*CTRL_DEG2RAD));
2356  }
2357  else if (version<3.0)
2358  {
2359  H0(1,3)=-0.0681;
2360 
2361  pushLink(new iKinLink( 0.0, 0.0, -M_PI/2.0, M_PI/2.0, -44.0*CTRL_DEG2RAD, 132.0*CTRL_DEG2RAD));
2362  pushLink(new iKinLink( 0.0, 0.0, -M_PI/2.0, M_PI/2.0, -119.0*CTRL_DEG2RAD, 17.0*CTRL_DEG2RAD));
2363  pushLink(new iKinLink(-0.0009175, -0.234545, M_PI/2.0, -M_PI/2.0, -79.0*CTRL_DEG2RAD, 79.0*CTRL_DEG2RAD));
2364  pushLink(new iKinLink( -0.2005, 0.0, M_PI, M_PI/2.0, -125.0*CTRL_DEG2RAD, 0.0*CTRL_DEG2RAD));
2365  pushLink(new iKinLink( 0.0, 0.0, -M_PI/2.0, 0.0, -42.0*CTRL_DEG2RAD, 21.0*CTRL_DEG2RAD));
2366  pushLink(new iKinLink( -0.06805, -0.0035, 0.0, 0.0, -24.0*CTRL_DEG2RAD, 24.0*CTRL_DEG2RAD));
2367  }
2368  else // version>=3.0
2369  {
2370  H0.zero();
2371  H0(0,0)=1.0;
2372  H0(1,2)=1.0;
2373  H0(2,1)=-1.0;
2374  H0(0,3)=-0.0216;
2375  H0(1,3)=-0.07365;
2376  H0(2,3)=-0.0605;
2377  H0(3,3)=1.0;
2378 
2379  pushLink(new iKinLink( 0.009, 0.0, -M_PI/2.0, M_PI/2.0, -40.0*CTRL_DEG2RAD, 125.0*CTRL_DEG2RAD));
2380  pushLink(new iKinLink( -0.015, -0.0106, -M_PI/2.0, M_PI/2.0, 5.0*CTRL_DEG2RAD, 111.0*CTRL_DEG2RAD));
2381  pushLink(new iKinLink( 0.015, -0.2668, M_PI/2.0, -M_PI/2.0, -62.5*CTRL_DEG2RAD, 62.5*CTRL_DEG2RAD));
2382  pushLink(new iKinLink(-0.255988, 0.0062, M_PI, M_PI/2.0, -100.0*CTRL_DEG2RAD, 0.0*CTRL_DEG2RAD));
2383  pushLink(new iKinLink( -0.035, 0.0, -M_PI/2.0, 0.0, -40.0*CTRL_DEG2RAD, 40.0*CTRL_DEG2RAD));
2384  pushLink(new iKinLink( -0.0532, 0.0, 0.0, 0.0, -20.0*CTRL_DEG2RAD, 20.0*CTRL_DEG2RAD));
2385  }
2386  }
2387 
2388  setH0(H0);
2389 }
2390 
2391 
2392 /************************************************************************/
2393 bool iCubLeg::alignJointsBounds(const deque<IControlLimits*> &lim)
2394 {
2395  if (lim.size()<1)
2396  return false;
2397 
2398  IControlLimits &limLeg=*lim[0];
2399 
2400  unsigned int iLeg;
2401  double min, max;
2402 
2403  for (iLeg=0; iLeg<getN(); iLeg++)
2404  {
2405  if (!limLeg.getLimits(iLeg,&min,&max))
2406  return false;
2407 
2408  (*this)[iLeg].setMin(CTRL_DEG2RAD*min);
2409  (*this)[iLeg].setMax(CTRL_DEG2RAD*max);
2410  }
2411 
2412  return true;
2413 }
2414 
2415 
2416 /************************************************************************/
2418 {
2419  allocate(getType());
2420 }
2421 
2422 
2423 /************************************************************************/
2424 iCubEye::iCubEye(const string &_type) : iKinLimb(_type)
2425 {
2426  allocate(_type);
2427 }
2428 
2429 
2430 /************************************************************************/
2431 void iCubEye::allocate(const string &_type)
2432 {
2433  string eye;
2434  size_t underscore=getType().find('_');
2435  if (underscore!=string::npos)
2436  {
2437  eye=getType().substr(0,underscore);
2438  version=strtod(getType().substr(underscore+2).c_str(),NULL);
2439  }
2440  else
2441  {
2442  eye=getType();
2443  version=1.0;
2444  }
2445 
2446  Matrix H0(4,4);
2447  H0.zero();
2448  H0(0,1)=-1.0;
2449  H0(1,2)=-1.0;
2450  H0(2,0)=1.0;
2451  H0(3,3)=1.0;
2452  setH0(H0);
2453 
2454  if (eye=="right")
2455  {
2456  if (version<2.0)
2457  {
2458  pushLink(new iKinLink( 0.032, 0.0, M_PI/2.0, 0.0, -22.0*CTRL_DEG2RAD, 84.0*CTRL_DEG2RAD));
2459  pushLink(new iKinLink( 0.0, -0.0055, M_PI/2.0, -M_PI/2.0, -39.0*CTRL_DEG2RAD, 39.0*CTRL_DEG2RAD));
2460  pushLink(new iKinLink(0.00231, -0.1933, -M_PI/2.0, -M_PI/2.0, -59.0*CTRL_DEG2RAD, 59.0*CTRL_DEG2RAD));
2461  pushLink(new iKinLink( 0.033, 0.0, M_PI/2.0, M_PI/2.0, -40.0*CTRL_DEG2RAD, 30.0*CTRL_DEG2RAD));
2462  pushLink(new iKinLink( 0.0, 0.001, -M_PI/2.0, -M_PI/2.0, -70.0*CTRL_DEG2RAD, 60.0*CTRL_DEG2RAD));
2463  pushLink(new iKinLink( -0.054, 0.0825, -M_PI/2.0, M_PI/2.0, -55.0*CTRL_DEG2RAD, 55.0*CTRL_DEG2RAD));
2464  pushLink(new iKinLink( 0.0, 0.034, -M_PI/2.0, 0.0, -35.0*CTRL_DEG2RAD, 15.0*CTRL_DEG2RAD));
2465  pushLink(new iKinLink( 0.0, 0.0, M_PI/2.0, -M_PI/2.0, -50.0*CTRL_DEG2RAD, 50.0*CTRL_DEG2RAD));
2466  }
2467  else if (version<3.0)
2468  {
2469  pushLink(new iKinLink( 0.032, 0.0, M_PI/2.0, 0.0, -22.0*CTRL_DEG2RAD, 84.0*CTRL_DEG2RAD));
2470  pushLink(new iKinLink( 0.0, -0.0055, M_PI/2.0, -M_PI/2.0, -39.0*CTRL_DEG2RAD, 39.0*CTRL_DEG2RAD));
2471  pushLink(new iKinLink( 0.0, -0.2233, -M_PI/2.0, -M_PI/2.0, -59.0*CTRL_DEG2RAD, 59.0*CTRL_DEG2RAD));
2472  pushLink(new iKinLink( 0.0095, 0.0, M_PI/2.0, M_PI/2.0, -40.0*CTRL_DEG2RAD, 30.0*CTRL_DEG2RAD));
2473  pushLink(new iKinLink( 0.0, 0.0, -M_PI/2.0, -M_PI/2.0, -70.0*CTRL_DEG2RAD, 60.0*CTRL_DEG2RAD));
2474  pushLink(new iKinLink(-0.0509, 0.08205, -M_PI/2.0, M_PI/2.0, -55.0*CTRL_DEG2RAD, 55.0*CTRL_DEG2RAD));
2475  pushLink(new iKinLink( 0.0, 0.034, -M_PI/2.0, 0.0, -35.0*CTRL_DEG2RAD, 15.0*CTRL_DEG2RAD));
2476  pushLink(new iKinLink( 0.0, 0.0, M_PI/2.0, -M_PI/2.0, -50.0*CTRL_DEG2RAD, 50.0*CTRL_DEG2RAD));
2477  }
2478  else // version>=3.0
2479  {
2480  H0.zero();
2481  H0(0,2)=1.0;
2482  H0(1,1)=-1.0;
2483  H0(2,0)=1.0;
2484  H0(3,3)=1.0;
2485  setH0(H0);
2486 
2487  pushLink(new iKinLink( 0.0725, 0.0, -M_PI/2.0, 0.0, -20.0*CTRL_DEG2RAD, 20.0*CTRL_DEG2RAD));
2488  pushLink(new iKinLink( 0.0, 0.0, M_PI/2.0, -M_PI/2.0, -15.0*CTRL_DEG2RAD, 40.0*CTRL_DEG2RAD));
2489  pushLink(new iKinLink(-0.00486151, -0.26377, -M_PI/2.0, M_PI, -40.0*CTRL_DEG2RAD, 40.0*CTRL_DEG2RAD));
2490  pushLink(new iKinLink( 0.0095, 0.0, M_PI/2.0, M_PI/2.0, -30.0*CTRL_DEG2RAD, 22.0*CTRL_DEG2RAD));
2491  pushLink(new iKinLink( 0.0, 0.0, -M_PI/2.0, -M_PI/2.0, -20.0*CTRL_DEG2RAD, 20.0*CTRL_DEG2RAD));
2492  pushLink(new iKinLink( -0.0509, 0.08205, -M_PI/2.0, M_PI/2.0, -45.0*CTRL_DEG2RAD, 45.0*CTRL_DEG2RAD));
2493  pushLink(new iKinLink( 0.0, 0.034, -M_PI/2.0, 0.0, -35.0*CTRL_DEG2RAD, 15.0*CTRL_DEG2RAD));
2494  pushLink(new iKinLink( 0.0, 0.0, M_PI/2.0, -M_PI/2.0, -50.0*CTRL_DEG2RAD, 50.0*CTRL_DEG2RAD));
2495  }
2496  }
2497  else // left
2498  {
2499  if (version<2.0)
2500  {
2501  pushLink(new iKinLink( 0.032, 0.0, M_PI/2.0, 0.0, -22.0*CTRL_DEG2RAD, 84.0*CTRL_DEG2RAD));
2502  pushLink(new iKinLink( 0.0, -0.0055, M_PI/2.0, -M_PI/2.0, -39.0*CTRL_DEG2RAD, 39.0*CTRL_DEG2RAD));
2503  pushLink(new iKinLink(0.00231, -0.1933, -M_PI/2.0, -M_PI/2.0, -59.0*CTRL_DEG2RAD, 59.0*CTRL_DEG2RAD));
2504  pushLink(new iKinLink( 0.033, 0.0, M_PI/2.0, M_PI/2.0, -40.0*CTRL_DEG2RAD, 30.0*CTRL_DEG2RAD));
2505  pushLink(new iKinLink( 0.0, 0.001, -M_PI/2.0, -M_PI/2.0, -70.0*CTRL_DEG2RAD, 60.0*CTRL_DEG2RAD));
2506  pushLink(new iKinLink( -0.054, 0.0825, -M_PI/2.0, M_PI/2.0, -55.0*CTRL_DEG2RAD, 55.0*CTRL_DEG2RAD));
2507  pushLink(new iKinLink( 0.0, -0.034, -M_PI/2.0, 0.0, -35.0*CTRL_DEG2RAD, 15.0*CTRL_DEG2RAD));
2508  pushLink(new iKinLink( 0.0, 0.0, M_PI/2.0, -M_PI/2.0, -50.0*CTRL_DEG2RAD, 50.0*CTRL_DEG2RAD));
2509  }
2510  else if (version<3.0)
2511  {
2512  pushLink(new iKinLink( 0.032, 0.0, M_PI/2.0, 0.0, -22.0*CTRL_DEG2RAD, 84.0*CTRL_DEG2RAD));
2513  pushLink(new iKinLink( 0.0, -0.0055, M_PI/2.0, -M_PI/2.0, -39.0*CTRL_DEG2RAD, 39.0*CTRL_DEG2RAD));
2514  pushLink(new iKinLink( 0.0, -0.2233, -M_PI/2.0, -M_PI/2.0, -59.0*CTRL_DEG2RAD, 59.0*CTRL_DEG2RAD));
2515  pushLink(new iKinLink( 0.0095, 0.0, M_PI/2.0, M_PI/2.0, -40.0*CTRL_DEG2RAD, 30.0*CTRL_DEG2RAD));
2516  pushLink(new iKinLink( 0.0, 0.0, -M_PI/2.0, -M_PI/2.0, -70.0*CTRL_DEG2RAD, 60.0*CTRL_DEG2RAD));
2517  pushLink(new iKinLink(-0.0509, 0.08205, -M_PI/2.0, M_PI/2.0, -55.0*CTRL_DEG2RAD, 55.0*CTRL_DEG2RAD));
2518  pushLink(new iKinLink( 0.0, -0.034, -M_PI/2.0, 0.0, -35.0*CTRL_DEG2RAD, 15.0*CTRL_DEG2RAD));
2519  pushLink(new iKinLink( 0.0, 0.0, M_PI/2.0, -M_PI/2.0, -50.0*CTRL_DEG2RAD, 50.0*CTRL_DEG2RAD));
2520  }
2521  else // version>=3.0
2522  {
2523  H0.zero();
2524  H0(0,2)=1.0;
2525  H0(1,1)=-1.0;
2526  H0(2,0)=1.0;
2527  H0(3,3)=1.0;
2528  setH0(H0);
2529 
2530  pushLink(new iKinLink( 0.0725, 0.0, -M_PI/2.0, 0.0, -20.0*CTRL_DEG2RAD, 20.0*CTRL_DEG2RAD));
2531  pushLink(new iKinLink( 0.0, 0.0, M_PI/2.0, -M_PI/2.0, -15.0*CTRL_DEG2RAD, 40.0*CTRL_DEG2RAD));
2532  pushLink(new iKinLink(-0.00486151, -0.26377, -M_PI/2.0, M_PI, -40.0*CTRL_DEG2RAD, 40.0*CTRL_DEG2RAD));
2533  pushLink(new iKinLink( 0.0095, 0.0, M_PI/2.0, M_PI/2.0, -30.0*CTRL_DEG2RAD, 22.0*CTRL_DEG2RAD));
2534  pushLink(new iKinLink( 0.0, 0.0, -M_PI/2.0, -M_PI/2.0, -20.0*CTRL_DEG2RAD, 20.0*CTRL_DEG2RAD));
2535  pushLink(new iKinLink( -0.0509, 0.08205, -M_PI/2.0, M_PI/2.0, -45.0*CTRL_DEG2RAD, 45.0*CTRL_DEG2RAD));
2536  pushLink(new iKinLink( 0.0, -0.034, -M_PI/2.0, 0.0, -35.0*CTRL_DEG2RAD, 15.0*CTRL_DEG2RAD));
2537  pushLink(new iKinLink( 0.0, 0.0, M_PI/2.0, -M_PI/2.0, -50.0*CTRL_DEG2RAD, 50.0*CTRL_DEG2RAD));
2538  }
2539  }
2540 
2541  if (is_version(version, 2.8))
2542  {
2543  Matrix HN = yarp::math::eye(4, 4);
2544  HN(2, 3) = -0.01;
2545  setHN(HN);
2546  }
2547 
2548  blockLink(0,0.0);
2549  blockLink(1,0.0);
2550  blockLink(2,0.0);
2551 }
2552 
2553 
2554 /************************************************************************/
2555 bool iCubEye::alignJointsBounds(const deque<IControlLimits*> &lim)
2556 {
2557  if (lim.size()<2)
2558  return false;
2559 
2560  IControlLimits &limTorso=*lim[0];
2561  IControlLimits &limHead =*lim[1];
2562 
2563  unsigned int iTorso;
2564  unsigned int iHead;
2565  double min, max;
2566 
2567  for (iTorso=0; iTorso<3; iTorso++)
2568  {
2569  if (!limTorso.getLimits(iTorso,&min,&max))
2570  return false;
2571 
2572  if (version<3.0)
2573  {
2574  (*this)[2-iTorso].setMin(CTRL_DEG2RAD*min);
2575  (*this)[2-iTorso].setMax(CTRL_DEG2RAD*max);
2576  }
2577  else
2578  {
2579  (*this)[iTorso].setMin(CTRL_DEG2RAD*min);
2580  (*this)[iTorso].setMax(CTRL_DEG2RAD*max);
2581  }
2582  }
2583 
2584  for (iHead=0; iHead<getN()-iTorso; iHead++)
2585  {
2586  if (!limHead.getLimits(iHead,&min,&max))
2587  return false;
2588 
2589  (*this)[iTorso+iHead].setMin(CTRL_DEG2RAD*min);
2590  (*this)[iTorso+iHead].setMax(CTRL_DEG2RAD*max);
2591  }
2592 
2593  return true;
2594 }
2595 
2596 
2597 /************************************************************************/
2599 {
2600  allocate(getType());
2601 }
2602 
2603 
2604 /************************************************************************/
2605 iCubEyeNeckRef::iCubEyeNeckRef(const string &_type) : iCubEye(_type)
2606 {
2607  allocate(_type);
2608 }
2609 
2610 
2611 /************************************************************************/
2612 void iCubEyeNeckRef::allocate(const string &_type)
2613 {
2614  rmLink(0);
2615  rmLink(0);
2616  rmLink(0);
2617 
2618  delete linkList[0];
2619  delete linkList[1];
2620  delete linkList[2];
2621 
2622  linkList.erase(linkList.begin(),linkList.begin()+2);
2623 }
2624 
2625 
2626 /************************************************************************/
2628 {
2629  allocate(getType());
2630 }
2631 
2632 
2633 /************************************************************************/
2634 iCubHeadCenter::iCubHeadCenter(const string &_type) : iCubEye(_type)
2635 {
2636  allocate(_type);
2637 }
2638 
2639 
2640 /************************************************************************/
2641 void iCubHeadCenter::allocate(const string &_type)
2642 {
2643  // change DH parameters
2644  (*this)[getN()-2].setD(0.0);
2645 
2646  // block last two links
2647  blockLink(getN()-2,0.0);
2648  blockLink(getN()-1,0.0);
2649 }
2650 
2651 
2652 /************************************************************************/
2654 {
2655  allocate(getType());
2656 }
2657 
2658 
2659 /************************************************************************/
2661 {
2662  allocate(_type);
2663 }
2664 
2665 
2666 /************************************************************************/
2667 void iCubInertialSensor::allocate(const string &_type)
2668 {
2669  if (getType().size()>1)
2670  version=strtod(getType().substr(1).c_str(),NULL);
2671  else
2672  version=1.0;
2673 
2674  Matrix H0(4,4);
2675  H0.zero();
2676  H0(3,3)=1.0;
2677 
2678  if (version<2.0)
2679  {
2680  H0(0,1)=-1.0;
2681  H0(1,2)=-1.0;
2682  H0(2,0)=1.0;
2683 
2684  type="v1";
2685  pushLink(new iKinLink( 0.032, 0.0, M_PI/2.0, 0.0, -22.0*CTRL_DEG2RAD, 84.0*CTRL_DEG2RAD));
2686  pushLink(new iKinLink( 0.0, -0.0055, M_PI/2.0, -M_PI/2.0, -39.0*CTRL_DEG2RAD, 39.0*CTRL_DEG2RAD));
2687  pushLink(new iKinLink(0.00231, -0.1933, -M_PI/2.0, -M_PI/2.0, -59.0*CTRL_DEG2RAD, 59.0*CTRL_DEG2RAD));
2688  pushLink(new iKinLink( 0.033, 0.0, M_PI/2.0, M_PI/2.0, -40.0*CTRL_DEG2RAD, 30.0*CTRL_DEG2RAD));
2689  pushLink(new iKinLink( 0.0, 0.001, -M_PI/2.0, -M_PI/2.0, -70.0*CTRL_DEG2RAD, 60.0*CTRL_DEG2RAD));
2690  pushLink(new iKinLink( 0.0225, 0.1005, -M_PI/2.0, M_PI/2.0, -55.0*CTRL_DEG2RAD, 55.0*CTRL_DEG2RAD));
2691  }
2692  else if (version<3.0)
2693  {
2694  H0(0,1)=-1.0;
2695  H0(1,2)=-1.0;
2696  H0(2,0)=1.0;
2697 
2698  pushLink(new iKinLink( 0.032, 0.0, M_PI/2.0, 0.0, -22.0*CTRL_DEG2RAD, 84.0*CTRL_DEG2RAD));
2699  pushLink(new iKinLink( 0.0, -0.0055, M_PI/2.0, -M_PI/2.0, -39.0*CTRL_DEG2RAD, 39.0*CTRL_DEG2RAD));
2700  pushLink(new iKinLink( 0.0, -0.2233, -M_PI/2.0, -M_PI/2.0, -59.0*CTRL_DEG2RAD, 59.0*CTRL_DEG2RAD));
2701  pushLink(new iKinLink( 0.0095, 0.0, M_PI/2.0, M_PI/2.0, -40.0*CTRL_DEG2RAD, 30.0*CTRL_DEG2RAD));
2702  pushLink(new iKinLink( 0.0, 0.0, -M_PI/2.0, -M_PI/2.0, -70.0*CTRL_DEG2RAD, 60.0*CTRL_DEG2RAD));
2703  pushLink(new iKinLink( 0.0185, 0.1108, -M_PI/2.0, M_PI/2.0, -55.0*CTRL_DEG2RAD, 55.0*CTRL_DEG2RAD));
2704  }
2705  else // version>=3.0
2706  {
2707  H0(0,2)=1.0;
2708  H0(1,1)=-1.0;
2709  H0(2,0)=1.0;
2710 
2711  pushLink(new iKinLink( 0.0725, 0.0, -M_PI/2.0, 0.0, -20.0*CTRL_DEG2RAD, 20.0*CTRL_DEG2RAD));
2712  pushLink(new iKinLink( 0.0, 0.0, M_PI/2.0, -M_PI/2.0, -15.0*CTRL_DEG2RAD, 40.0*CTRL_DEG2RAD));
2713  pushLink(new iKinLink(-0.00486151, -0.26377, -M_PI/2.0, M_PI, -40.0*CTRL_DEG2RAD, 40.0*CTRL_DEG2RAD));
2714  pushLink(new iKinLink( 0.0095, 0.0, M_PI/2.0, M_PI/2.0, -30.0*CTRL_DEG2RAD, 22.0*CTRL_DEG2RAD));
2715  pushLink(new iKinLink( 0.0, 0.0, -M_PI/2.0, -M_PI/2.0, -20.0*CTRL_DEG2RAD, 20.0*CTRL_DEG2RAD));
2716  pushLink(new iKinLink( 0.0185, 0.1108, -M_PI/2.0, M_PI/2.0, -45.0*CTRL_DEG2RAD, 45.0*CTRL_DEG2RAD));
2717  }
2718 
2719  setH0(H0);
2720 
2721  // T_nls (see http://wiki.icub.org/wiki/ICubInertiaSensorKinematics )
2722  Matrix HN(4,4);
2723  HN.zero();
2724  HN(0,0)=1.0;
2725  HN(2,1)=1.0;
2726  HN(1,2)=-1.0;
2727  HN(2,3)=0.0066;
2728  HN(3,3)=1.0;
2729  setHN(HN);
2730 
2731  if (version>2.0)
2732  {
2733  Matrix HN=eye(4,4);
2734  HN(0,3)=0.0087;
2735  HN(1,3)=0.01795;
2736  HN(2,3)=-0.0105;
2737  setHN(getHN()*HN);
2738  }
2739 
2740  // further displacement for >= 2.6
2741  if (version>=2.6)
2742  {
2743  Matrix HN=zeros(4,4);
2744  HN(0,3)=0.0323779;
2745  HN(1,3)=-0.0139537;
2746  HN(2,3)=0.072;
2747  HN(1,0)=1.0;
2748  HN(0,1)=1.0;
2749  HN(2,2)=-1.0;
2750  HN(3,3)=1.0;
2751  setHN(getHN()*HN);
2752  }
2753 }
2754 
2755 
2756 /************************************************************************/
2757 bool iCubInertialSensor::alignJointsBounds(const deque<IControlLimits*> &lim)
2758 {
2759  if (lim.size()<2)
2760  return false;
2761 
2762  IControlLimits &limTorso=*lim[0];
2763  IControlLimits &limHead =*lim[1];
2764 
2765  unsigned int iTorso;
2766  unsigned int iHead;
2767  double min, max;
2768 
2769  for (iTorso=0; iTorso<3; iTorso++)
2770  {
2771  if (!limTorso.getLimits(iTorso,&min,&max))
2772  return false;
2773 
2774  if (version<3.0)
2775  {
2776  (*this)[2-iTorso].setMin(CTRL_DEG2RAD*min);
2777  (*this)[2-iTorso].setMax(CTRL_DEG2RAD*max);
2778  }
2779  else
2780  {
2781  (*this)[iTorso].setMin(CTRL_DEG2RAD*min);
2782  (*this)[iTorso].setMax(CTRL_DEG2RAD*max);
2783  }
2784  }
2785 
2786  // only the neck
2787  for (iHead=0; iHead<3; iHead++)
2788  {
2789  if (!limHead.getLimits(iHead,&min,&max))
2790  return false;
2791 
2792  (*this)[iTorso+iHead].setMin(CTRL_DEG2RAD*min);
2793  (*this)[iTorso+iHead].setMax(CTRL_DEG2RAD*max);
2794  }
2795 
2796  return true;
2797 }
2798 
2799 
2800 /************************************************************************/
2802 {
2803  allocate(getType());
2804 }
2805 
2806 
2807 /************************************************************************/
2809 {
2810  allocate(_type);
2811 }
2812 
2813 
2814 /************************************************************************/
2815 void iCubInertialSensorWaist::allocate(const string &_type)
2816 {
2817  if (getType().size()>1)
2818  version=strtod(getType().substr(1).c_str(),NULL);
2819  else
2820  version=2.7;
2821 
2822  Matrix H0(4,4);
2823  H0.zero();
2824 
2825  H0(0,1)=-0.5;
2826  H0(0,2)=0.866025;
2827  H0(0,3)=0.0906529;
2828 
2829  H0(1,0)=-1.0;
2830  H0(1,3)=-0.011;
2831 
2832  H0(2,1)=-0.866025;
2833  H0(2,2)=-0.5;
2834  H0(2,3)=-0.112309;
2835 
2836  H0(3,3)=1.0;
2837  setH0(H0);
2838 }
iCub::iKin::iCubArm::alignJointsBounds
virtual bool alignJointsBounds(const std::deque< yarp::dev::IControlLimits * > &lim)
Alignes the Arm joints bounds with current values set aboard the iCub.
Definition: iKinFwd.cpp:1766
h
h
Definition: compute_ekf_sym.m:23
iCub::iKin::iKinChain::getH
yarp::sig::Matrix getH()
Returns the rigid roto-translation matrix from the root reference frame to the end-effector frame in ...
Definition: iKinFwd.cpp:786
iKinFwd.h
iCub
This file contains the definition of unique IDs for the body parts and the skin parts of the robot.
Definition: emotionInterface.h:17
iCub::iKin::iCubInertialSensor::allocate
virtual void allocate(const std::string &_type)
Definition: iKinFwd.cpp:2667
iCub::iKin::iKinChain::dRotAng
yarp::sig::Vector dRotAng(const yarp::sig::Matrix &R, const yarp::sig::Matrix &dR)
Definition: iKinFwd.cpp:681
H
H
Definition: compute_ekf_fast.m:27
iCub::iKin::iCubLeg::version
double version
Definition: iKinFwd.h:1239
iCub::iKin::iKinChain::AnaJacobian
yarp::sig::Matrix AnaJacobian(const unsigned int i, unsigned int col)
Returns the analitical Jacobian of the ith link.
Definition: iKinFwd.cpp:919
iCub::iKin::iKinChain::RotAng
yarp::sig::Vector RotAng(const yarp::sig::Matrix &R)
Definition: iKinFwd.cpp:667
iCub::iKin::iCubFinger::allocate
virtual void allocate(const std::string &_type)
Definition: iKinFwd.cpp:1850
iCub::iKin::iKinChain::~iKinChain
virtual ~iKinChain()
Destructor.
Definition: iKinFwd.cpp:1308
iCub::iKin::iKinChain::isLinkBlocked
bool isLinkBlocked(const unsigned int i)
Queries whether the ith Link is blocked.
Definition: iKinFwd.cpp:491
iCub::iKin::iKinChain::Position
yarp::sig::Vector Position(const unsigned int i)
Returns the 3D position coordinates of ith Link.
Definition: iKinFwd.cpp:850
iCub::iKin::iCubFinger::clone
virtual void clone(const iCubFinger &finger)
Definition: iKinFwd.cpp:1830
iCub::iKin::iKinChain::N
unsigned int N
Definition: iKinFwd.h:357
iCub::iKin::notImplemented
void notImplemented(const unsigned int verbose)
Definition: iKinFwd.cpp:39
iCub::iKin::iCubLeg::iCubLeg
iCubLeg()
Default constructor.
Definition: iKinFwd.cpp:2264
iCub::iKin::iKinChain::EndEffPosition
yarp::sig::Vector EndEffPosition()
Returns the 3D coordinates of end-effector position.
Definition: iKinFwd.cpp:902
iCub::iKin::iKinLimb::iKinLimb
iKinLimb()
Default constructor.
Definition: iKinFwd.cpp:1323
e
e
Definition: compute_ekf_fast.m:13
iCub::iKin::iKinChain::operator=
iKinChain & operator=(const iKinChain &c)
Copies a Chain object into the current one.
Definition: iKinFwd.cpp:298
iCub::iKin::iCubTorso::alignJointsBounds
virtual bool alignJointsBounds(const std::deque< yarp::dev::IControlLimits * > &lim)
Alignes the Torso joints bounds with current values set aboard the iCub.
Definition: iKinFwd.cpp:1605
iCub::iKin::iKinChain
Definition: iKinFwd.h:354
iCub::iKin::iKinChain::Hessian_ij
yarp::sig::Vector Hessian_ij(const unsigned int i, const unsigned int j)
Returns the 6x1 vector where is the forward kinematic function and is the DOF couple.
Definition: iKinFwd.cpp:1102
offset
degrees offset
Definition: sine.m:4
iCub::iKin::iCubLeg::allocate
virtual void allocate(const std::string &_type)
Definition: iKinFwd.cpp:2278
CTRL_DEG2RAD
#define CTRL_DEG2RAD
Definition: XSensMTx.cpp:26
iCub::iKin::iKinChain::hash
std::deque< unsigned int > hash
Definition: iKinFwd.h:367
iCub::iKin::iKinChain::operator--
iKinChain & operator--(int)
Removes a Link from the bottom of the Chain.
Definition: iKinFwd.cpp:393
iCub::iKin::iKinChain::releaseLink
bool releaseLink(const unsigned int i)
Releases the ith Link.
Definition: iKinFwd.cpp:471
iCub::iKin::iKinChain::setH0
bool setH0(const yarp::sig::Matrix &_H0)
Sets H0, the rigid roto-translation matrix from the root reference frame to the 0th frame.
Definition: iKinFwd.cpp:570
iCub::iKin::iKinChain::popLink
void popLink()
Removes a Link from the bottom of the Chain.
Definition: iKinFwd.cpp:383
strain::dsp::fsc::min
const FSC min
Definition: strain.h:49
iCub::iKin::iCubInertialSensorWaist::version
double version
Definition: iKinFwd.h:1404
iCub::iKin::iCubFinger::hand
std::string hand
Definition: iKinFwd.h:1124
iCub::iKin::iKinChain::clear
void clear()
Removes all Links.
Definition: iKinFwd.cpp:361
iCub::iKin::iKinChain::d2RotAng
yarp::sig::Vector d2RotAng(const yarp::sig::Matrix &R, const yarp::sig::Matrix &dRi, const yarp::sig::Matrix &dRj, const yarp::sig::Matrix &d2R)
Definition: iKinFwd.cpp:694
iCub::iKin::iCubArm::iCubArm
iCubArm()
Default constructor.
Definition: iKinFwd.cpp:1637
iCub::iKin::iKinChain::Pose
yarp::sig::Vector Pose(const unsigned int i, const bool axisRep=true)
Returns the coordinates of ith Link.
Definition: iKinFwd.cpp:811
iCub::iKin::iCubFinger
Definition: iKinFwd.h:1121
iCub::iKin::iKinLimb::linkList
std::deque< iKinLink * > linkList
Definition: iKinFwd.h:876
iCub::iKin::iKinLimb::fromLinksProperties
bool fromLinksProperties(const yarp::os::Property &options)
Initializes the Limb from a list of properties wherein links parameters are specified.
Definition: iKinFwd.cpp:1395
iCub::iKin::iCubLeg::alignJointsBounds
virtual bool alignJointsBounds(const std::deque< yarp::dev::IControlLimits * > &lim)
Alignes the Leg joints bounds with current values set aboard the iCub.
Definition: iKinFwd.cpp:2393
yarp::dev
Definition: DebugInterfaces.h:52
iCub::iKin::iKinChain::setAng
yarp::sig::Vector setAng(const yarp::sig::Vector &q)
Sets the free joint angles to values of q[i].
iCub::iKin::iCubEyeNeckRef::allocate
virtual void allocate(const std::string &_type)
Definition: iKinFwd.cpp:2612
iCub::iKin::iCubEye
Definition: iKinFwd.h:1273
iCub::iKin::iCubArm::version
double version
Definition: iKinFwd.h:1084
iCub::iKin::iKinChain::setConstraint
void setConstraint(unsigned int i, bool _constrained)
Sets the constraint status of ith link.
Definition: iKinFwd.h:517
iCub::iKin::iCubEye::allocate
virtual void allocate(const std::string &_type)
Definition: iKinFwd.cpp:2431
iCub::iKin::iKinChain::clone
virtual void clone(const iKinChain &c)
Definition: iKinFwd.cpp:272
iCub::iKin::iKinChain::curr_q
yarp::sig::Vector curr_q
Definition: iKinFwd.h:362
iCub::iKin::iKinLimb::pushLink
void pushLink(iKinLink &l)
Definition: iKinFwd.h:897
iCub::iKin::iCubEyeNeckRef::iCubEyeNeckRef
iCubEyeNeckRef()
Default constructor.
Definition: iKinFwd.cpp:2598
strain::dsp::fsc::max
const FSC max
Definition: strain.h:48
iCub::iKin::iKinLimb
Definition: iKinFwd.h:873
iCub::iKin::iKinChain::iKinChain
iKinChain()
Default constructor.
Definition: iKinFwd.cpp:264
iCub::iKin::iKinLimb::rmLink
bool rmLink(const unsigned int i)
Definition: iKinFwd.h:896
iCub::iKin::iKinChain::getAng
yarp::sig::Vector getAng()
Returns the current free joint angles values.
Definition: iKinFwd.cpp:619
iCub::iKin::iKinLimb::toLinksProperties
bool toLinksProperties(yarp::os::Property &options)
Provides the links attributes listed in a property object.
Definition: iKinFwd.cpp:1451
CTRL_RAD2DEG
#define CTRL_RAD2DEG
Definition: XSensMTx.cpp:25
iCub::iKin::iKinChain::DJacobian
yarp::sig::Matrix DJacobian(const yarp::sig::Vector &dq)
Compute the time derivative of the geometric Jacobian.
iCub::iKin::iCubInertialSensor::alignJointsBounds
virtual bool alignJointsBounds(const std::deque< yarp::dev::IControlLimits * > &lim)
Alignes the Inertial Sensor joints bounds with current values set aboard the iCub.
Definition: iKinFwd.cpp:2757
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
iCub::iKin::iKinLimb::operator=
iKinChain & operator=(const iKinChain &c)
Definition: iKinFwd.h:890
iCub::iKin::iKinChain::fastHessian_ij
yarp::sig::Vector fastHessian_ij(const unsigned int i, const unsigned int j)
Returns the 6x1 vector where is the forward kinematic function and is the DOF couple.
Definition: iKinFwd.cpp:1125
iCub::iKin::iKinLimb::setMatrixToProperties
virtual void setMatrixToProperties(yarp::os::Property &options, const std::string &tag, yarp::sig::Matrix &H)
Definition: iKinFwd.cpp:1382
iCub::iKin
Definition: iKinFwd.h:71
iCub::iKin::iKinChain::build
virtual void build()
Definition: iKinFwd.cpp:522
iCub::iKin::iKinChain::GeoJacobian
yarp::sig::Matrix GeoJacobian()
Returns the geometric Jacobian of the end-effector.
Definition: iKinFwd.cpp:1056
iCub::iKin::iKinChain::H0
yarp::sig::Matrix H0
Definition: iKinFwd.h:360
iCub::iKin::iCubHeadCenter::allocate
virtual void allocate(const std::string &_type)
Definition: iKinFwd.cpp:2641
iCub::iKin::iKinLimb::allocate
virtual void allocate(const std::string &_type)
Definition: iKinFwd.cpp:1523
iCub::iKin::iKinChain::addLink
bool addLink(const unsigned int i, iKinLink &l)
Adds a Link at the position ith within the Chain.
Definition: iKinFwd.cpp:307
iCub::iKin::iKinChain::pushLink
void pushLink(iKinLink &l)
Adds a Link at the bottom of the Chain.
Definition: iKinFwd.cpp:351
iCub::iKin::iCubFinger::fingers_abduction_max
double fingers_abduction_max
Definition: iKinFwd.h:1127
iCub::iKin::iKinChain::dispose
virtual void dispose()
Definition: iKinFwd.cpp:1315
iCub::iKin::iKinChain::getN
unsigned int getN() const
Returns the number of Links belonging to the Chain.
Definition: iKinFwd.h:550
iCub::iKin::iKinLimb::clone
virtual void clone(const iKinLimb &limb)
Definition: iKinFwd.cpp:1530
iCub::ctrl
Definition: adaptWinPolyEstimator.h:37
iCub::iKin::iKinLimb::dispose
virtual void dispose()
Definition: iKinFwd.cpp:1542
iCub::iKin::iKinChain::hash_dof
std::deque< unsigned int > hash_dof
Definition: iKinFwd.h:368
n
int n
Definition: debugFunctions.cpp:58
iCub::iKin::iKinChain::prepareForHessian
void prepareForHessian()
Prepares computation for a successive call to fastHessian_ij().
Definition: iKinFwd.cpp:1110
iCub::iKin::iCubInertialSensorWaist::allocate
virtual void allocate(const std::string &_type)
Definition: iKinFwd.cpp:2815
iCub::iKin::iKinChain::HN
yarp::sig::Matrix HN
Definition: iKinFwd.h:361
iCub::iKin::iCubTorso::allocate
virtual void allocate(const std::string &_type)
Definition: iKinFwd.cpp:1568
iCub::iKin::iKinChain::quickList
std::deque< iKinLink * > quickList
Definition: iKinFwd.h:365
iCub::iKin::iKinLimb::getMatrixFromProperties
virtual void getMatrixFromProperties(const yarp::os::Property &options, const std::string &tag, yarp::sig::Matrix &H)
Definition: iKinFwd.cpp:1359
iCub::iKin::iKinLimb::type
std::string type
Definition: iKinFwd.h:877
iCub::iKin::iCubFinger::alignJointsBounds
virtual bool alignJointsBounds(const std::deque< yarp::dev::IControlLimits * > &lim)
Alignes the finger joints bounds with current values set aboard the iCub.
Definition: iKinFwd.cpp:2021
y
y
Definition: show_eyes_axes.m:21
x
x
Definition: compute_ekf_sym.m:21
iCub::iKin::iCubFinger::iCubFinger
iCubFinger()
Default constructor.
Definition: iKinFwd.cpp:1809
string
string(REPLACE "-rdynamic" "" CMAKE_SHARED_LIBRARY_LINK_CXX_FLAGS "${CMAKE_SHARED_LIBRARY_LINK_CXX_FLAGS}") include_directories($
Definition: CMakeLists.txt:9
iCub::iKin::iCubEye::alignJointsBounds
virtual bool alignJointsBounds(const std::deque< yarp::dev::IControlLimits * > &lim)
Alignes the Eye joints bounds with current values set aboard the iCub.
Definition: iKinFwd.cpp:2555
iCub::iKin::iCubInertialSensorWaist::iCubInertialSensorWaist
iCubInertialSensorWaist()
Default constructor.
Definition: iKinFwd.cpp:2801
iCub::iKin::iCubInertialSensor::iCubInertialSensor
iCubInertialSensor()
Default constructor.
Definition: iKinFwd.cpp:2653
iCub::iKin::iKinChain::verbose
unsigned int verbose
Definition: iKinFwd.h:359
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
iCub::iKin::iKinChain::DOF
unsigned int DOF
Definition: iKinFwd.h:358
iCub::iKin::iKinChain::hess_Jlnk
yarp::sig::Matrix hess_Jlnk
Definition: iKinFwd.h:371
iCub::ctrl::cross
yarp::sig::Vector cross(const yarp::sig::Matrix &A, int colA, const yarp::sig::Matrix &B, int colB)
iCub::iKin::iKinLimb::~iKinLimb
virtual ~iKinLimb()
Destructor.
Definition: iKinFwd.cpp:1516
iCub::iKin::iKinChain::rmLink
bool rmLink(const unsigned int i)
Removes the ith Link from the Chain.
Definition: iKinFwd.cpp:329
iCub::iKin::iCubInertialSensor::version
double version
Definition: iKinFwd.h:1367
iCub::iKin::iCubFinger::version
std::string version
Definition: iKinFwd.h:1126
A
A
Definition: sine.m:16
iCub::iKin::iKinChain::setBlockingValue
bool setBlockingValue(const unsigned int i, double Ang)
Changes the value of the ith blocked Link.
Definition: iKinFwd.cpp:422
iCub::iKin::iKinChain::setVerbosity
void setVerbosity(unsigned int _verbose)
Sets the verbosity level of the Chain.
Definition: iKinFwd.h:538
iCub::iKin::iKinChain::setAllLinkVerbosity
void setAllLinkVerbosity(unsigned int _verbose)
Sets the verbosity level of all Links belonging to the Chain.
Definition: iKinFwd.cpp:514
iCub::iKin::iCubFinger::finger
std::string finger
Definition: iKinFwd.h:1125
iCub::iKin::iKinChain::allList
std::deque< iKinLink * > allList
Definition: iKinFwd.h:364
v
static int v
Definition: iCub_Sim.cpp:42
iCub::iKin::iCubHeadCenter::iCubHeadCenter
iCubHeadCenter()
Default constructor.
Definition: iKinFwd.cpp:2627
iCub::iKin::iKinChain::EndEffPose
yarp::sig::Vector EndEffPose(const bool axisRep=true)
Returns the coordinates of end-effector.
Definition: iKinFwd.cpp:858
iCub::iKin::iKinChain::blockLink
bool blockLink(const unsigned int i, double Ang)
Blocks the ith Link at the a certain value of its joint angle.
Definition: iKinFwd.cpp:402
iCub::iKin::iCubEye::version
double version
Definition: iKinFwd.h:1276
iCub::iKin::iCubArm::allocate
virtual void allocate(const std::string &_type)
Definition: iKinFwd.cpp:1651
iCub::iKin::iCubFinger::getChainJoints
virtual bool getChainJoints(const yarp::sig::Vector &motorEncoders, yarp::sig::Vector &chainJoints)
Retrieves the vector of actual finger's joint values (to be used in conjuction with the iKinLimb meth...
iCub::iKin::iCubTorso::iCubTorso
iCubTorso()
Default constructor.
Definition: iKinFwd.cpp:1554
iCub::iKin::iKinChain::operator<<
iKinChain & operator<<(iKinLink &l)
Adds a Link at the bottom of the Chain.
Definition: iKinFwd.cpp:374
zeros
zeros(2, 2) eye(2
iCub::iKin::iKinChain::setAllConstraints
void setAllConstraints(bool _constrained)
Sets the constraint status of all chain links.
Definition: iKinFwd.cpp:506
iCub::iKin::iKinLimb::getType
std::string getType() const
Returns the Limb type as a string.
Definition: iKinFwd.h:1019
iCub::iKin::iCubTorso::version
double version
Definition: iKinFwd.h:1048
M_PI
#define M_PI
Definition: XSensMTx.cpp:24
iCub::iKin::iCubFinger::operator=
iCubFinger & operator=(const iCubFinger &finger)
Copies a Finger object into the current one.
Definition: iKinFwd.cpp:1839
iCub::iKin::iKinChain::hess_J
yarp::sig::Matrix hess_J
Definition: iKinFwd.h:370
iCub::iKin::is_version
bool is_version(const double v1, const double v2)
Definition: iKinFwd.cpp:31
iCub::iKin::version_epsilon
constexpr double version_epsilon
Definition: iKinFwd.cpp:30
iCub::iKin::iCubEye::iCubEye
iCubEye()
Default constructor.
Definition: iKinFwd.cpp:2417