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