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