iCub-main
iDyn.cpp
Go to the documentation of this file.
1 /*
2 * Copyright (C) 2010-2011 RobotCub Consortium, European Commission FP6 Project IST-004370
3 * Author: Serena Ivaldi, Matteo Fumagalli
4 * email: serena.ivaldi@iit.it, matteo.fumagalli@iit.it
5 * website: www.robotcub.org
6 * Permission is granted to copy, distribute, and/or modify this program
7 * under the terms of the GNU General Public License, version 2 or any
8 * later version published by the Free Software Foundation.
9 *
10 * A copy of the license can be found at
11 * http://www.robotcub.org/icub/license/gpl.txt
12 *
13 * This program is distributed in the hope that it will be useful, but
14 * WITHOUT ANY WARRANTY; without even the implied warranty of
15 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General
16 * Public License for more details
17 */
18 
19 #include <cmath>
20 #include <cstdio>
21 #include <iostream>
22 #include <iomanip>
23 
24 #include <yarp/os/Log.h>
25 #include <iCub/iDyn/iDyn.h>
26 #include <iCub/ctrl/math.h>
27 
28 
29 using namespace std;
30 using namespace yarp::os;
31 using namespace yarp::dev;
32 using namespace yarp::sig;
33 using namespace yarp::math;
34 using namespace iCub::ctrl;
35 using namespace iCub::iKin;
36 using namespace iCub::iDyn;
37 
38 #define mra 1.0
39 #define mla 1.0
40 #define mhd 1.0
41 
42 #define NO_JOINT_COSTRAINTS
43 
44 //#define mra 1e-11
45 //#define mla 1e-11
46 //#define mhd 1.0
47 //================================
48 //
49 // I DYN HELPERS
50 //
51 //================================
52 
53 
54 void iCub::iDyn::notImplemented(const unsigned int verbose)
55 {
56  if(verbose) yError("iDyn: error: not implemented \n");
57 }
58 
59 void iCub::iDyn::notImplemented(const unsigned int verbose, const string &msg)
60 {
61  if(verbose) yError("iDyn: error: not implemented \n %s \n",msg.c_str());
62 }
63 
64 void iCub::iDyn::workInProgress(const unsigned int verbose, const std::string &msg)
65 {
66  if(verbose) yError("iDyn: warning: this method/class is still under development. Please do not use it! \n %s \n",msg.c_str());
67 }
68 
69 bool iCub::iDyn::asWrench(Vector &w, const Vector &f, const Vector &m)
70 {
71  w.resize(6); w.zero();
72  if((f.length()==3)||(m.length()==3))
73  {
74  w[0]=f[0]; w[1]=f[1]; w[2]=f[2];
75  w[3]=m[0]; w[4]=m[1]; w[5]=m[2];
76  return true;
77  }
78  else
79  {
80  yError("iDyn: error in calling asWrench(), wrong sized vectors: (%d,%d) instead of (3,3). return wrench set automatically as zero.\n",(int)f.length(),(int)m.length());
81  return false;
82  }
83 }
84 
85 Vector iCub::iDyn::asWrench(const Vector &f, const Vector &m)
86 {
87  Vector w(6); w.zero();
88  if((f.length()==3)||(m.length()==3))
89  {
90  w[0]=f[0]; w[1]=f[1]; w[2]=f[2];
91  w[3]=m[0]; w[4]=m[1]; w[5]=m[2];
92  }
93  else
94  {
95  yError("iDyn: error in calling asWrench(), wrong sized vectors: (%d,%d) instead of (3,3). return wrench set automatically as zero.\n",(int)f.length(),(int)m.length());
96  }
97  return w;
98 }
99 
100 bool iCub::iDyn::asForceMoment(const Vector &w, Vector &f, Vector &m)
101 {
102  f.resize(3); f.zero();
103  m.resize(3); m.zero();
104 
105  if(w.length()==6)
106  {
107  f[0]=w[0]; f[1]=w[1]; f[2]=w[2];
108  m[0]=w[3]; m[1]=w[4]; m[2]=w[5];
109  return true;
110  }
111  else
112  {
113  yError("iDyn: error in calling asForceMoment(), wrong sized vector: (%d) instead of (6). return force/moment set automatically as zero.\n",(int)w.length());
114  return false;
115  }
116 }
117 
118 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
119 
120 
121 //================================
122 //
123 // I DYN LINK
124 //
125 //================================
126 
127 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
128 iDynLink::iDynLink(double _A, double _D, double _Alpha, double _Offset, double _Min, double _Max)
129 : iKinLink( _A, _D, _Alpha, _Offset, _Min, _Max)
130 {
131  zero();
132 }
133 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
134 iDynLink::iDynLink(const double _m, const Matrix &_HC, const Matrix &_I, double _A, double _D, double _Alpha, double _Offset, double _Min, double _Max)
135 : iKinLink( _A, _D, _Alpha, _Offset, _Min, _Max)
136 {
137  zero();
138  m = _m;
139  setInertia(_I);
140  setCOM(_HC);
141 }
142 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
143 iDynLink::iDynLink(const double _m, const Vector &_C, const Matrix &_I, double _A, double _D, double _Alpha, double _Offset, double _Min, double _Max)
144 : iKinLink( _A, _D, _Alpha, _Offset, _Min, _Max)
145 {
146  zero();
147  m = _m;
148  setInertia(_I);
149  setCOM(_C);
150 }
151 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
152 iDynLink::iDynLink(const double _m, const double _rCx, const double _rCy, const double _rCz, const double Ixx, const double Ixy, const double Ixz, const double Iyy, const double Iyz, const double Izz, double _A, double _D, double _Alpha, double _Offset, double _Min, double _Max)
153 : iKinLink( _A, _D, _Alpha, _Offset, _Min, _Max)
154 {
155  zero();
156  m = _m;
157  setInertia(Ixx,Ixy,Ixz,Iyy,Iyz,Izz);
158  setCOM(_rCx,_rCy,_rCz);
159 }
160 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
161 iDynLink::iDynLink(const iDynLink &c)
162 : iKinLink(c)
163 {
164  clone(c);
165 }
166 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
167 iDynLink &iDynLink::operator=(const iDynLink &c)
168 {
169  clone(c);
170  return *this;
171 }
172 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
173 void iDynLink::clone(const iDynLink &c)
174 {
175  iKinLink::clone(c);
176 
177  m = c.getMass();
178  I = c.getInertia();
179  HC = c.getCOM();
180  RC = c.getRC();
181  rc = c.getrC();
182  rc_proj = rc*RC;
183  Im = c.getIm();
184  Fv = c.getFv();
185  Fs = c.getFs();
186  kr = c.getKr();
187  dq = c.getDAng();
188  ddq = c.getD2Ang();
189  w = c.getW();
190  dw = c.getdW();
191  dwM = c.getdWM();
192  dp = c.getLinVel();
193  dpC = c.getLinVelC();
194  ddp = c.getLinAcc();
195  ddpC = c.getLinAccC();
196  F = c.getForce();
197  Mu = c.getMoment();
198  Tau = c.getTorque();
199  H_store_valid = false;
200 }
201 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
202 
203 
204  //~~~~~~~~~~~~~~~~~~~~~~
205  // set methods
206  //~~~~~~~~~~~~~~~~~~~~~~
207 
208 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
209 bool iDynLink::setDynamicParameters(const double _m, const yarp::sig::Matrix &_HC, const yarp::sig::Matrix &_I, const double _kr, const double _Fv, const double _Fs, const double _Im)
210 {
211  m = _m;
212  kr = _kr;
213  Fv = _Fv;
214  Fs = _Fs;
215  Im = _Im;
216  return setInertia(_I) && setCOM(_HC);
217 }
218 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
219 bool iDynLink::setDynamicParameters(const double _m, const yarp::sig::Matrix &_HC, const yarp::sig::Matrix &_I)
220 {
221  m = _m;
222  return setInertia(_I) && setCOM(_HC);
223 }
224 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
225 bool iDynLink::setStaticParameters(const double _m, const yarp::sig::Matrix &_HC)
226 {
227  m = _m;
228  return setCOM(_HC);
229 }
230 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
231 bool iDynLink::setInertia(const yarp::sig::Matrix &_I)
232 {
233  if( (_I.rows()==3)&&(_I.cols()==3) )
234  {
235  I = _I;
236  return true;
237  }
238  else
239  {
240  I.resize(3,3); I.zero();
241  if(verbose) yError("iDynLink: error in setting Inertia due to wrong matrix size: (%zu,%zu) instead of (3,3). Inertia matrix now set automatically to zero. \n",_I.rows(),_I.cols());
242  return false;
243  }
244 }
245 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
246 void iDynLink::setInertia(const double Ixx, const double Ixy, const double Ixz, const double Iyy, const double Iyz, const double Izz)
247 {
248  I.resize(3,3); I.zero();
249  I(0,0) = Ixx;
250  I(0,1) = I(1,0) = Ixy;
251  I(0,2) = I(2,0) = Ixz;
252  I(1,1) = Iyy;
253  I(1,2) = I(2,1) = Iyz;
254  I(2,2) = Izz;
255 }
256 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
257 void iDynLink::setMass(const double _m)
258 {
259  m = _m;
260 }
261 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
262 double iDynLink::setAng(const double _teta)
263 {
264  H_store_valid = false; // invalidate the stored rototranslation matrix H
265  return iKinLink::setAng(_teta);
266 }
267 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
268 double iDynLink::setDAng(const double _dteta)
269 {
270  dq = _dteta;
271  return dq;
272 }
273 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
274 double iDynLink::setD2Ang(const double _ddteta)
275 {
276  ddq = _ddteta;
277  return ddq;
278 }
279 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
280 void iDynLink::setAngPosVelAcc(const double _teta,const double _dteta,const double _ddteta)
281 {
282  setAng(_teta);
283  setDAng(_dteta);
284  setD2Ang(_ddteta);
285 }
286 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
287 bool iDynLink::setCOM(const yarp::sig::Matrix &_HC)
288 {
289  if((_HC.rows()==4) && (_HC.cols()==4))
290  {
291  HC = _HC;
292  RC = HC.submatrix(0,2,0,2);
293  rc = HC.getCol(3).subVector(0,2);
294  rc_proj = rc*RC;
295  return true;
296  }
297  else
298  {
299  HC = eye(4,4);
300  RC = eye(3,3);
301  rc = zeros(3);
302  if(verbose)
303  yError("iDynLink: error in setting COM roto-translation due to wrong matrix size: (%zu,%zu) instead of (4,4). HC matrix now set automatically as eye.\n",_HC.rows(),_HC.cols());
304  return false;
305  }
306 }
307 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
308 bool iDynLink::setCOM(const yarp::sig::Vector &_rC)
309 {
310  if(_rC.length()==3)
311  {
312  HC = eye(4,4);
313  HC(0,3) = _rC(0);
314  HC(1,3) = _rC(1);
315  HC(2,3) = _rC(2);
316  RC = eye(3,3);
317  rc = _rC;
318  rc_proj = rc*RC;
319  return true;
320  }
321  else
322  {
323  if(verbose)
324  yError("iDynLink error, cannot set distance from COM due to wrong sized vector: %d instead of 3 \n",(int)_rC.length());
325  return false;
326  }
327 }
328 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
329 void iDynLink::setCOM(const double _rCx, const double _rCy, const double _rCz)
330 {
331  HC = eye(4,4);
332  HC(0,3) = _rCx;
333  HC(1,3) = _rCy;
334  HC(2,3) = _rCz;
335  RC = eye(3,3);
336  rc = cat(_rCx, _rCy, _rCz);
337  rc_proj = rc*RC;
338 }
339 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
340 bool iDynLink::setForce(const yarp::sig::Vector &_F)
341 {
342  if( _F.length()==3)
343  {
344  F = _F;
345  return true;
346  }
347  else
348  {
349  if(verbose)
350  yError("iDynLink error: cannot set forces due to wrong size: %d instead of 3.\n",(int)_F.length());
351  return false;
352  }
353 }
354 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
355 bool iDynLink::setMoment(const yarp::sig::Vector &_Mu)
356 {
357  if(_Mu.length()==3)
358  {
359  Mu = _Mu;
360  return true;
361  }
362  else
363  {
364  if(verbose)
365  yError("iDynLink error, cannot set moments due to wrong size: %d instead of 3. \n",(int)_Mu.length());
366  return false;
367  }
368 }
369 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
370 bool iDynLink::setForceMoment(const yarp::sig::Vector &_F, const yarp::sig::Vector &_Mu)
371 {
372  return setForce(_F) && setMoment(_Mu);
373 }
374 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
375 void iDynLink::setTorque(const double _Tau)
376 {
377  Tau = _Tau;
378 }
379 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
380 void iDynLink::zero()
381 {
382  m = 0.0;
383  dq = 0.0; ddq = 0.0;
384  I.resize(3,3); I.zero();
385  w.resize(3); w.zero();
386  dw.resize(3); dw.zero();
387  dwM.resize(3); dwM.zero();
388  dp.resize(3); dp.zero();
389  dpC.resize(3); dpC.zero();
390  ddp.resize(3); ddp.zero();
391  ddpC.resize(3); ddpC.zero();
392  F.resize(3); F.zero();
393  Mu.resize(3); Mu.zero();
394  Tau = 0.0;
395  Im = 0.0; kr = 0.0; Fv = 0.0; Fs = 0.0;
396  HC = eye(4,4); RC = eye(3,3); rc = zeros(3);
397  H_store = eye(4,4); R_store = eye(3,3); r_store = zeros(3);
398  H_store_valid = false;
399 }
400 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
401 
402  //~~~~~~~~~~~~~~~~~~~~~~
403  // get methods
404  //~~~~~~~~~~~~~~~~~~~~~~
405 
406 const Matrix& iDynLink::getInertia() const {return I;}
407 double iDynLink::getMass() const {return m;}
408 double iDynLink::getIm() const {return Im;}
409 double iDynLink::getKr() const {return kr;}
410 double iDynLink::getFs() const {return Fs;}
411 double iDynLink::getFv() const {return Fv;}
412 const Matrix& iDynLink::getCOM() const {return HC;}
413 double iDynLink::getDAng() const {return dq;}
414 double iDynLink::getD2Ang() const {return ddq;}
415 const Vector& iDynLink::getW() const {return w;}
416 const Vector& iDynLink::getdW() const {return dw;}
417 const Vector& iDynLink::getdWM() const {return dwM;}
418 const Vector& iDynLink::getLinVel() const {return dp;}
419 const Vector& iDynLink::getLinVelC() const {return dpC;}
420 const Vector& iDynLink::getLinAcc() const {return ddp;}
421 const Vector& iDynLink::getLinAccC() const {return ddpC;}
422 const Vector& iDynLink::getForce() const {return F;}
423 const Vector& iDynLink::getMoment() const {return Mu;}
424 double iDynLink::getTorque() const {return Tau;}
425 const Matrix& iDynLink::getRC() const {return RC;}
426 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
427 void iDynLink::updateHstore()
428 {
429  if(!H_store_valid)
430  {
431  H_store = iKinLink::getH(true);
432  R_store = H_store.submatrix(0,2,0,2);
433  r_store = H_store.subcol(0,3,3);
434  r_proj_store = r_store*R_store;
435  H_store_valid = true;
436  }
437 }
438 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
439 const Matrix& iDynLink::getR()
440 {
441  updateHstore();
442  return R_store;
443 }
444 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
445 const Matrix& iDynLink::getH()
446 {
447  updateHstore();
448  return H_store;
449 }
450 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
451 const Vector& iDynLink::getr(bool proj)
452 {
453  updateHstore();
454  if(proj==false)
455  return r_store;
456  return r_proj_store;
457 }
458 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
459 const Vector& iDynLink::getrC(bool proj) const
460 {
461  if(proj==false)
462  return rc;
463  return rc_proj;
464 }
465 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
466 
467 
468 
469 //================================
470 //
471 // I DYN CHAIN
472 //
473 //================================
474 
475 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
476 iDynChain::iDynChain()
477 : iKinChain()
478 {
479  NE=NULL;
480  setIterMode(KINFWD_WREBWD);
481 }
482 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
483 void iDynChain::clone(const iDynChain &c)
484 {
485  iKinChain::clone(c);
486  curr_dq = c.curr_dq;
487  curr_ddq = c.curr_ddq;
488  iterateMode_kinematics = c.iterateMode_kinematics;
489  iterateMode_wrench = c.iterateMode_wrench;
490  NE = c.NE;
491 }
492 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
493 void iDynChain::build()
494 {
495  iKinChain::build();
496  if(DOF)
497  {
498  curr_dq = getDAng();
499  curr_ddq = getD2Ang();
500  }
501 }
502 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
503 iDynChain::iDynChain(const iDynChain &c)
504 {
505  clone(c);
506 }
507 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
508 void iDynChain::dispose()
509 {
510  iKinChain::dispose();
511  if(NE)
512  {
513  delete NE;
514  NE=NULL;
515  }
516 }
517 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
518 iDynChain::~iDynChain()
519 {
520  dispose();
521 }
522 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
523 iDynChain &iDynChain::operator=(const iDynChain &c)
524 {
525  clone(c);
526  return *this;
527 }
528 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
529 Vector iDynChain::setAng(const Vector &q)
530 {
531  if (DOF==0)
532  {
533  if (verbose)
534  yError("setAng() failed since DOF==0\n");
535 
536  return Vector(0);
537  }
538 
539  int sz=(q.length()>(int)DOF)?DOF:q.length();
540  for (int i=0; i<sz; i++) // cast to iDynLink so calling setAng invalidates the H matrix
541  curr_q[i] = dynamic_cast<iDynLink*>(quickList[hash_dof[i]])->setAng(q[i]);
542 
543  return curr_q;
544 }
545 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
546 Vector iDynChain::setDAng(const Vector &dq)
547 {
548  if(!DOF)
549  return Vector(0);
550 
551  curr_dq.resize(DOF);
552 
553  if(dq.length()>=(int)DOF)
554  {
555  for(unsigned int i=0; i<DOF; i++)
556  curr_dq[i]=quickList[hash_dof[i]]->setDAng(dq[i]);
557  }
558  else
559  if(verbose)
560  yError("iDynChain error: setVel() failed: %d joint angles needed \n",DOF);
561 
562  return curr_dq;
563 }
564 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
565 Vector iDynChain::setD2Ang(const Vector &ddq)
566 {
567  if(!DOF)
568  return Vector(0);
569 
570  curr_ddq.resize(DOF);
571 
572  if(ddq.length()>=(int)DOF)
573  {
574  for(unsigned int i=0; i<DOF; i++)
575  curr_ddq[i]=quickList[hash_dof[i]]->setD2Ang(ddq[i]);
576  }
577  else
578  if(verbose)
579  yError("iDynChain error: setD2Ang() failed: %d joint angles needed \n",DOF);
580 
581  return curr_ddq;
582 }
583 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
584 Vector iDynChain::getDAng()
585 {
586  if(!DOF)
587  return Vector(0);
588 
589  curr_dq.resize(DOF);
590 
591  for(unsigned int i=0; i<DOF; i++)
592  curr_dq[i]=quickList[hash_dof[i]]->getDAng();
593 
594  return curr_dq;
595 }
596 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
597 Vector iDynChain::getD2Ang()
598 {
599  if(!DOF)
600  return Vector(0);
601 
602  curr_ddq.resize(DOF);
603 
604  for(unsigned int i=0; i<DOF; i++)
605  curr_ddq[i]=quickList[hash_dof[i]]->getD2Ang();
606 
607  return curr_ddq;
608 }
609 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
610 Vector iDynChain::getJointBoundMin()
611 {
612  if(!DOF)
613  return Vector(0);
614 
615  Vector jointBounds(DOF);
616  for(unsigned int i=0; i<DOF; i++)
617  jointBounds[i]=quickList[hash_dof[i]]->getMin();
618  return jointBounds;
619 }
620 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
621 Vector iDynChain::getJointBoundMax()
622 {
623  if(!DOF)
624  return Vector(0);
625 
626  Vector jointBounds(DOF);
627  for(unsigned int i=0; i<DOF; i++)
628  jointBounds[i]=quickList[hash_dof[i]]->getMax();
629  return jointBounds;
630 }
631 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
632 double iDynChain::setAng(const unsigned int i, double _Ang)
633 {
634  double res=0.0;
635 
636  if (i<N)
637  res = dynamic_cast<iDynLink*>(allList[i])->setAng(_Ang);
638  else if (verbose)
639  yError("setAng() failed due to out of range index: %d>=%d\n",i,N);
640 
641  return res;
642 }
643 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
644 double iDynChain::setDAng(const unsigned int i, double _dq)
645 {
646  if(i<N)
647  return allList[i]->setDAng(_dq);
648  else
649  {
650  if(verbose) yError("iDynChain error: setVel() failed due to out of range index: %d >= %d \n",i,N);
651  return 0.0;
652 
653  }
654 }
655 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
656 double iDynChain::setD2Ang(const unsigned int i, double _ddq)
657 {
658  if(i<N)
659  return allList[i]->setD2Ang(_ddq);
660  else
661  {
662  if(verbose) yError("iDynChain error: setD2Ang() failed due to out of range index: %d >= %d \n",i,N);
663  return 0.0;
664  }
665 }
666 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
667 double iDynChain::getDAng(const unsigned int i)
668 {
669  if(i<N)
670  return allList[i]->getDAng();
671  else
672  {
673  if(verbose) yError("iDynChain error: getDAng() failed due to out of range index: %d >= %d \n",i,N);
674  return 0.0;
675  }
676 }
677 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
678 double iDynChain::getD2Ang(const unsigned int i)
679 {
680  if(i<N)
681  return allList[i]->getD2Ang();
682  else
683  {
684  if(verbose) yError("iDynChain error: getD2Ang() failed due to out of range index: %d >= %d \n",i,N);
685  return 0.0;
686  }
687 }
688 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
689 Vector iDynChain::getLinVel(const unsigned int i) const
690 {
691  if(i<N)
692  return allList[i]->getLinVel();
693  else
694  {
695  if(verbose) yError("iDynChain error: getLinVel() failed due to out of range index: %d >= %d \n",i,N);
696  return Vector(0);
697  }
698 }
699 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
700 Vector iDynChain::getLinVelCOM(const unsigned int i) const
701 {
702  if(i<N)
703  return allList[i]->getLinVelC();
704  else
705  {
706  if(verbose) yError("iDynChain error: getLinVelCOM() failed due to out of range index: %d >= %d \n",i,N);
707  return Vector(0);
708  }
709 }
710 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
711 Vector iDynChain::getLinAcc(const unsigned int i) const
712 {
713  if(i<N)
714  return allList[i]->getLinAcc();
715  else
716  {
717  if(verbose) yError("iDynChain error: getLinAcc() failed due to out of range index: %d >= %d \n",i,N);
718  return Vector(0);
719  }
720 }
721 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
722 const Vector& iDynChain::getLinAccCOM(const unsigned int i) const
723 {
724  if(i<N)
725  return allList[i]->getLinAccC();
726  else
727  {
728  if(verbose) yError("iDynChain error: getLinAccCOM() failed due to out of range index: %d >= %d \n",i,N);
729  return zero0;
730  }
731 }
732 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
733 Vector iDynChain::getAngVel(const unsigned int i) const
734 {
735  if(i<N)
736  return allList[i]->getW();
737  else
738  {
739  if(verbose) yError("iDynChain error: getAngVel() failed due to out of range index: %d >= %d \n",i,N);
740  return Vector(0);
741  }
742 }
743 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
744 Vector iDynChain::getAngAcc(const unsigned int i) const
745 {
746  if(i<N)
747  return allList[i]->getdW();
748  else
749  {
750  if(verbose) yError("iDynChain error: getAngAcc() failed due to out of range index: %d >= %d \n",i,N);
751  return Vector(0);
752  }
753 }
754 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
755 iDynLink * iDynChain::refLink(const unsigned int i)
756 {
757  if(i<N)
758  return dynamic_cast<iDynLink *>(allList[i]);
759  else
760  {
761  if(verbose) yError("iDynChain error: refLink() failed due to out of range index: %d >= %d \n",i,N);
762  return NULL;
763  }
764 }
765 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
766 const Vector& iDynChain::getForce(const unsigned int iLink) const
767 {
768  if(iLink<N)
769  return allList[iLink]->getForce();
770  else
771  {
772  if(verbose) yError("iDynChain error: getForce() failed due to out of range index: %d >= %d \n",iLink,N);
773  return zero0;
774  }
775 }
776 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
777 const Vector& iDynChain::getMoment(const unsigned int iLink) const
778 {
779  if(iLink<N)
780  return allList[iLink]->getMoment();
781  else
782  {
783  if(verbose) yError("iDynChain error: getMoment() failed due to out of range index: %d >= %d \n",iLink,N);
784  return zero0;
785  }
786 }
787 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
788 double iDynChain::getTorque(const unsigned int iLink) const
789 {
790  if(iLink<N)
791  return allList[iLink]->getTorque();
792  else
793  {
794  if(verbose) yError("iDynChain error: getTorque() failed due to out of range index: %d >= %d \n",iLink,N);
795  return 0.0;
796  }
797 }
798 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
799 Vector iDynChain::getMasses() const
800 {
801  Vector ret(N); ret.zero();
802  for(unsigned int i=0;i<N;i++)
803  ret[i] = allList[i]->getMass();
804  return ret;
805 }
806 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
807 bool iDynChain::setMasses(Vector _m)
808 {
809  if(_m.length()==N)
810  {
811  for(unsigned int i=0; i<N; i++)
812  allList[i]->setMass(_m[i]);
813  return true;
814  }
815  else
816  {
817  if(verbose) yError("iDynChain error: setMasses() failed due to wrong vector size: %d instead of %d",(int)_m.length(),N);
818  return false;
819  }
820 }
821 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
822 double iDynChain::getMass(const unsigned int i) const
823 {
824  if(i<N)
825  return allList[i]->getMass();
826  else
827  {
828  if(verbose) yError("iDynChain error: getMass() failed due to out of range index: %d >= %d \n",i,N);
829  return 0.0;
830  }
831 }
832 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
833 bool iDynChain::setMass(const unsigned int i, const double _m)
834 {
835  if(i<N)
836  {
837  allList[i]->setMass(_m);
838  return true;
839  }
840  else
841  {
842  if(verbose) yError("iDynChain error: setMass() failed due to out of range index: %d >= %d \n",i,N);
843  return false;
844  }
845 }
846 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
847 Matrix iDynChain::getInertia(const unsigned int i) const
848 {
849  if(i<N)
850  return allList[i]->getInertia();
851  else
852  {
853  if(verbose)
854  yError("iDynChain: getInertia() failed due to out of range index: %d >= %d \n",i,N);
855  return Matrix(0,0);
856  }
857 }
858 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
859 Matrix iDynChain::getForces() const
860 {
861  Matrix ret(3,N);
862  for(unsigned int i=0;i<N;i++)
863  ret.setCol(i,allList[i]->getForce());
864  return ret;
865 }
866 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
867 Matrix iDynChain::getMoments() const
868 {
869  Matrix ret(3,N);
870  for(unsigned int i=0;i<N;i++)
871  ret.setCol(i,allList[i]->getMoment());
872  return ret;
873 }
874 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
875 Vector iDynChain::getTorques() const
876 {
877  Vector ret(N);
878  for(unsigned int i=0;i<N;i++)
879  ret[i]= allList[i]->getTorque();
880  return ret;
881 }
882 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
883 bool iDynChain::setDynamicParameters(const unsigned int i, const double _m, const Matrix &_HC, const Matrix &_I, const double _kr, const double _Fv, const double _Fs, const double _Im)
884 {
885  if(i<N)
886  return allList[i]->setDynamicParameters(_m,_HC,_I,_kr,_Fv,_Fs,_Im);
887  else
888  {
889  if(verbose) yError("iDynChain error: setDynamicParameters() failed due to out of range index: %d >= %d \n",i,N);
890  return false;
891  }
892 }
893 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
894 bool iDynChain::setDynamicParameters(const unsigned int i, const double _m, const Matrix &_HC, const Matrix &_I)
895 {
896  if(i<N)
897  return allList[i]->setDynamicParameters(_m,_HC,_I);
898  else
899  {
900  if(verbose) yError("iDynChain error: setDynamicParameters() failed due to out of range index: %d >= %d \n",i,N);
901  return false;
902  }
903 }
904 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
905 bool iDynChain::setStaticParameters(const unsigned int i, const double _m, const Matrix &_HC)
906 {
907  if(i<N)
908  return allList[i]->setStaticParameters(_m,_HC);
909  else
910  {
911  if(verbose) yError("iDynChain error: setStaticParameters() failed due to out of range index: %d >= %d \n",i,N);
912  return false;
913  }
914 }
915 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
916 void iDynChain::prepareNewtonEuler(const NewEulMode NewEulMode_s)
917 {
918  string info;
919  info = "[Chain] ";
920  char buffer[60];
921  int j = sprintf(buffer,"DOF=%d N=%d",DOF,N);
922  info.append(buffer);
923 
924  if( NE != NULL)
925  delete NE;
926  NE = new OneChainNewtonEuler(const_cast<iDynChain *>(this),info,NewEulMode_s,verbose);
927 }
928 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
929 bool iDynChain::computeNewtonEuler(const Vector &w0, const Vector &dw0, const Vector &ddp0, const Vector &F0, const Vector &Mu0 )
930 {
931  if( NE == NULL)
932  {
933  if(verbose)
934  {
935  yError("iDynChain error: trying to call computeNewtonEuler() without having prepared Newton-Euler method in the class. \n");
936  yError("iDynChain: prepareNewtonEuler() called autonomously in the default mode. \n");
937  }
938  prepareNewtonEuler();
939  }
940 
941  if((w0.length()==3)&&(dw0.length()==3)&&(ddp0.length()==3)&&(F0.length()==3)&&(Mu0.length()==3))
942  {
943  if(iterateMode_kinematics == FORWARD)
944  NE->ForwardKinematicFromBase(w0,dw0,ddp0);
945  else
946  NE->BackwardKinematicFromEnd(w0,dw0,ddp0);
947 
948  if(iterateMode_wrench == BACKWARD)
949  NE->BackwardWrenchFromEnd(F0,Mu0);
950  else
951  NE->ForwardWrenchFromBase(F0,Mu0);
952  return true;
953  }
954  else
955  {
956  if(verbose)
957  {
958  yError("iDynChain error: could not compute with Newton Euler due to wrong sized initializing vectors: \n");
959  yError(" w0,dw0,ddp0,Fend,Muend have size %d,%d,%d,%d,%d instead of 3,3,3,3,3 \n",(int)w0.length(),(int)dw0.length(),(int)ddp0.length(),(int)F0.length(),(int)Mu0.length());
960  }
961  return false;
962  }
963 }
964 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
965 bool iDynChain::computeNewtonEuler()
966 {
967  if( NE == NULL)
968  {
969  if(verbose)
970  {
971  yError("iDynChain error: trying to call computeNewtonEuler() without having prepared Newton-Euler method in the class. \n");
972  yError("iDynChain: prepareNewtonEuler() called autonomously in the default mode. \n");
973  yError("iDynChain: initNewtonEuler() called autonomously with default values. \n");
974  }
975  prepareNewtonEuler();
976  initNewtonEuler();
977  }
978 
979  if(iterateMode_kinematics == FORWARD)
980  NE->ForwardKinematicFromBase();
981  else
982  NE->BackwardKinematicFromEnd();
983 
984  if(iterateMode_wrench == BACKWARD)
985  NE->BackwardWrenchFromEnd();
986  else
987  NE->ForwardWrenchFromBase();
988 
989  return true;
990 }
991 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
992 void iDynChain::computeKinematicNewtonEuler()
993 {
994  if(iterateMode_kinematics == FORWARD)
995  NE->ForwardKinematicFromBase();
996  else
997  NE->BackwardKinematicFromEnd();
998 }
999 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
1000 void iDynChain::computeWrenchNewtonEuler()
1001 {
1002  if(iterateMode_wrench == BACKWARD)
1003  NE->BackwardWrenchFromEnd();
1004  else
1005  NE->ForwardWrenchFromBase();
1006 }
1007 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
1008 void iDynChain::getKinematicNewtonEuler(Vector &w, Vector &dw, Vector &ddp)
1009 {
1010  if( NE == NULL)
1011  {
1012  if(verbose)
1013  {
1014  yError("iDynChain error: trying to call getKinematicNewtonEuler() without having prepared Newton-Euler method in the class. \n");
1015  yError("iDynChain: prepareNewtonEuler() called autonomously in the default mode. \n");
1016  yError("iDynChain: initNewtonEuler() called autonomously with default values. \n");
1017  }
1018  prepareNewtonEuler();
1019  initNewtonEuler();
1020  }
1021 
1022  w.resize(3); dw.resize(3); ddp.resize(3); w=dw=ddp=0.0;
1023  if(iterateMode_kinematics == FORWARD)
1024  {
1025  //get kinematics from the end-effector
1026  NE->getVelAccEnd(w,dw,ddp);
1027  }
1028  else
1029  {
1030  //get kinematics from the base
1031  NE->getVelAccBase(w,dw,ddp);
1032  }
1033 
1034 }
1035 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
1036 void iDynChain::getFrameKinematic(unsigned int i, Vector &w, Vector &dw, Vector &ddp)
1037 {
1038  if( NE == NULL)
1039  {
1040  if(verbose)
1041  {
1042  yError("iDynChain error: trying to call getFrameKinematic() without having prepared Newton-Euler method in the class. \n");
1043  yError("iDynChain: prepareNewtonEuler() called autonomously in the default mode. \n");
1044  yError("iDynChain: initNewtonEuler() called autonomously with default values. \n");
1045  }
1046  prepareNewtonEuler();
1047  initNewtonEuler();
1048  }
1049 
1050  Vector dwM(3);dwM=0.0;
1051  Vector ddpC(3);ddpC=0.0;
1052  NE->getVelAccAfterForward(i,w,dw,dwM,ddp,ddpC);
1053 }
1054 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
1055 void iDynChain::getFrameWrench(unsigned int i, Vector &F, Vector &Mu)
1056 {
1057  if( NE == NULL)
1058  {
1059  if(verbose)
1060  {
1061  yError("iDynChain error: trying to call getFrameWrench() without having prepared Newton-Euler method in the class. \n");
1062  yError("iDynChain: prepareNewtonEuler() called autonomously in the default mode. \n");
1063  yError("iDynChain: initNewtonEuler() called autonomously with default values. \n");
1064  }
1065  prepareNewtonEuler();
1066  initNewtonEuler();
1067  }
1068 
1069  NE->getWrenchAfterForward(i,F,Mu);
1070 }
1071 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
1072 void iDynChain::getWrenchNewtonEuler(Vector &F, Vector &Mu)
1073 {
1074  if( NE == NULL)
1075  {
1076  if(verbose)
1077  {
1078  yError("iDynChain error: trying to call getWrenchNewtonEuler() without having prepared Newton-Euler method in the class. \n");
1079  yError("iDynChain: prepareNewtonEuler() called autonomously in the default mode. \n");
1080  yError("iDynChain: initNewtonEuler() called autonomously with default values. \n");
1081  }
1082  prepareNewtonEuler();
1083  initNewtonEuler();
1084  }
1085  F.resize(3); Mu.resize(3); F=Mu=0.0;
1086  if(iterateMode_wrench == BACKWARD)
1087  {
1088  //get wrench from the base
1089  NE->getWrenchBase(F,Mu);
1090  }
1091  else
1092  {
1093  //get wrench from the end-effector
1094  NE->getWrenchEnd(F,Mu);
1095  }
1096 
1097 }
1098 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
1099 bool iDynChain::initNewtonEuler()
1100 {
1101  Vector z3(3, 0.0);
1102  return initNewtonEuler(z3, z3, z3, z3, z3);
1103 }
1104 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
1105 bool iDynChain::initNewtonEuler(const Vector &w0, const Vector &dw0, const Vector &ddp0, const Vector &Fend, const Vector &Muend)
1106 {
1107  if( NE == NULL)
1108  {
1109  if(verbose)
1110  {
1111  yError("iDynChain error: trying to call initNewtonEuler() without having prepared Newton-Euler method in the class. \n");
1112  yError("iDynChain: prepareNewtonEuler() called autonomously in the default mode. \n");
1113  }
1114  prepareNewtonEuler();
1115  }
1116 
1117  if((w0.length()==3)&&(dw0.length()==3)&&(ddp0.length()==3)&&(Fend.length()==3)&&(Muend.length()==3))
1118  {
1119  bool ret=true;
1120 
1121  if(iterateMode_kinematics == FORWARD)
1122  ret = ret && NE->initKinematicBase(w0,dw0,ddp0);
1123  else
1124  ret = ret && NE->initKinematicEnd(w0,dw0,ddp0);
1125 
1126  if(iterateMode_wrench == BACKWARD)
1127  ret = ret && NE->initWrenchEnd(Fend,Muend);
1128  else
1129  ret = ret && NE->initWrenchBase(Fend,Muend);
1130 
1131  return ret;
1132  }
1133  else
1134  {
1135  if(verbose)
1136  {
1137  yError("iDynChain error: could not initialize Newton Euler due to wrong sized initializing vectors: ");
1138  yError(" w0,dw0,ddp0,Fend,Muend have size %d,%d,%d,%d,%d instead of 3,3,3,3,3 \n",(int)w0.length(),(int)dw0.length(),(int)ddp0.length(),(int)Fend.length(),(int)Muend.length());
1139  }
1140  return false;
1141  }
1142 }
1143 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
1144 bool iDynChain::initKinematicNewtonEuler(const Vector &w0, const Vector &dw0, const Vector &ddp0)
1145 {
1146  if(NE == NULL)
1147  {
1148  if(verbose)
1149  {
1150  yError("iDynChain error: trying to call initKinematicNewtonEuler() without having prepared Newton-Euler method in the class. \n");
1151  yError("iDynChain: prepareNewtonEuler() called autonomously in the default mode. \n");
1152  }
1153  prepareNewtonEuler();
1154  }
1155 
1156  if((w0.length()==3)&&(dw0.length()==3)&&(ddp0.length()==3))
1157  {
1158  if(iterateMode_kinematics == FORWARD)
1159  return NE->initKinematicBase(w0,dw0,ddp0);
1160  else
1161  return NE->initKinematicEnd(w0,dw0,ddp0);
1162  }
1163  else
1164  {
1165  if(verbose)
1166  {
1167  yError("iDynChain error: could not initialize Newton Euler due to wrong sized initializing vectors: ");
1168  yError(" w0,dw0,ddp0 have size %d,%d,%d instead of 3,3,3 \n",(int)w0.length(),(int)dw0.length(),(int)ddp0.length());
1169  }
1170  return false;
1171  }
1172 }
1173 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
1174 bool iDynChain::initWrenchNewtonEuler(const Vector &Fend, const Vector &Muend)
1175 {
1176  if( NE == NULL)
1177  {
1178  if(verbose)
1179  {
1180  yError("iDynChain error: trying to call initWrenchNewtonEuler() without having prepared Newton-Euler method in the class. \n");
1181  yError("iDynChain: prepareNewtonEuler() called autonomously in the default mode. \n");
1182  }
1183  prepareNewtonEuler();
1184  }
1185 
1186  if((Fend.length()==3)&&(Muend.length()==3))
1187  {
1188  if(iterateMode_wrench == BACKWARD)
1189  return NE->initWrenchEnd(Fend,Muend);
1190  else
1191  return NE->initWrenchBase(Fend,Muend);
1192  }
1193  else
1194  {
1195  if(verbose)
1196  {
1197  yError("iDynChain error: could not initialize Newton Euler due to wrong sized initializing vectors: ");
1198  yError(" Fend,Muend have size %d,%d instead of 3,3 \n",(int)Fend.length(),(int)Muend.length());
1199  }
1200  return false;
1201  }
1202 }
1203 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
1204 void iDynChain::setModeNewtonEuler(const NewEulMode mode)
1205 {
1206  if( NE == NULL)
1207  {
1208  if(verbose)
1209  {
1210  yError("iDynChain error: trying to call setModeNewtonEuler() without having prepared Newton-Euler method in the class. \n");
1211  yError("iDynChain: prepareNewtonEuler() called autonomously in the default mode. \n");
1212  }
1213  prepareNewtonEuler();
1214  }
1215 
1216  NE->setMode(mode);
1217  if(verbose) yInfo("iDynChain: Newton-Euler mode set to %s \n",NewEulMode_s[mode].c_str());
1218 }
1219 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
1220 
1221  //~~~~~~~~~~~~~~
1222  // plus get
1223  //~~~~~~~~~~~~~~
1224 
1225 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
1226 Matrix iDynChain::getForcesNewtonEuler() const
1227 {
1228  Matrix ret(3,N+2); ret.zero();
1229  if( NE != NULL)
1230  {
1231  for(unsigned int i=0;i<N+2;i++)
1232  ret.setCol(i,NE->neChain[i]->getForce());
1233  }
1234  else
1235  {
1236  if(verbose) yError("iDynChain error: trying to call getForcesNewtonEuler() without having prepared Newton-Euler. \n");
1237  }
1238 
1239  return ret;
1240 }
1241 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
1242 Matrix iDynChain::getMomentsNewtonEuler() const
1243 {
1244  Matrix ret(3,N+2); ret.zero();
1245  if( NE != NULL)
1246  {
1247  for(unsigned int i=0;i<N+2;i++)
1248  ret.setCol(i,NE->neChain[i]->getMoment());
1249  }
1250  else
1251  {
1252  if(verbose) yError("iDynChain error: trying to call getMomentsNewtonEuler() without having prepared Newton-Euler. \n");
1253  }
1254  return ret;
1255 }
1256 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
1257 Vector iDynChain::getTorquesNewtonEuler() const
1258 {
1259  Vector ret(N); ret.zero();
1260  if( NE != NULL)
1261  {
1262  for(unsigned int i=0;i<N;i++)
1263  ret[i] = NE->neChain[i+1]->getTorque();
1264  }
1265  else
1266  {
1267  if(verbose) yError("iDynChain error: trying to call getTorquesNewtonEuler() without having prepared Newton-Euler. \n");
1268  }
1269  return ret;
1270 }
1271 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
1272 Vector iDynChain::getForceMomentEndEff() const
1273 {
1274  Vector ret(6); ret.zero();
1275  Vector f = allList[N-1]->getForce();
1276  Vector m = allList[N-1]->getMoment();
1277  ret[0]=f[0]; ret[1]=f[1]; ret[2]=f[2];
1278  ret[3]=m[0]; ret[4]=m[1]; ret[5]=m[2];
1279  return ret;
1280 }
1281 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
1282 void iDynChain::setIterModeKinematic(const ChainIterationMode _iterateMode_kinematics)
1283 {
1284  iterateMode_kinematics = _iterateMode_kinematics;
1285 }
1286 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
1287 void iDynChain::setIterModeWrench(const ChainIterationMode _iterateMode_wrench)
1288 {
1289  iterateMode_wrench = _iterateMode_wrench;
1290 }
1291 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
1292 ChainIterationMode iDynChain::getIterModeKinematic() const
1293 {
1294  return iterateMode_kinematics;
1295 }
1296 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
1297 ChainIterationMode iDynChain::getIterModeWrench() const
1298 {
1299  return iterateMode_wrench;
1300 }
1301 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
1302 void iDynChain::setIterMode(const ChainComputationMode mode)
1303 {
1304  switch(mode)
1305  {
1306  case KINFWD_WREBWD: setIterModeKinematic(FORWARD); setIterModeWrench(BACKWARD);
1307  break;
1308  case KINFWD_WREFWD: setIterModeKinematic(FORWARD); setIterModeWrench(FORWARD);
1309  break;
1310  case KINBWD_WREBWD: setIterModeKinematic(BACKWARD); setIterModeWrench(BACKWARD);
1311  break;
1312  case KINBWD_WREFWD: setIterModeKinematic(BACKWARD); setIterModeWrench(FORWARD);
1313  break;
1314  default:
1315  if(verbose) yError("iDynChain error: in setIterMode() could not set iteration mode due to unexisting mode \n");
1316  }
1317 }
1318 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
1319 
1320  //-----------
1321  // jacobian
1322  //-----------
1323 
1324 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
1325 Matrix iDynChain::computeGeoJacobian(const unsigned int iLinkN, const Matrix &Pn)
1326 {
1327  if(DOF==0)
1328  {
1329  if(verbose) yError("iDynChain: computeGeoJacobian() failed since DOF==0 \n");
1330  return Matrix(0,0);
1331  }
1332  if(iLinkN>=N)
1333  {
1334  if(verbose) yError("iDynChain: computeGeoJacobian() failed due to out of range indexes: from 0 to %d >= %d \n", iLinkN, N);
1335  return Matrix(0,0);
1336  }
1337 
1338  // the jacobian size is linkN+1: eg, index=2, Njoints=0,1,2=3
1339  Matrix J(6, iLinkN+1 );J.zero();
1340  Matrix Z;
1341  Vector w;
1342 
1343  deque<Matrix> intH;
1344  intH.push_back(H0);
1345  for (unsigned int i=0; i<iLinkN; i++)
1346  intH.push_back(intH[i]*allList[i]->getH(true));
1347 
1348  for (unsigned int i=0; i<iLinkN; i++)
1349  {
1350  unsigned int j=hash[i];
1351  Z=intH[j];
1352  w=cross(Z,2,Pn-Z,3);
1353  J(0,j)=w[0];
1354  J(1,j)=w[1];
1355  J(2,j)=w[2];
1356  J(3,j)=Z(0,2);
1357  J(4,j)=Z(1,2);
1358  J(5,j)=Z(2,2);
1359  }
1360  return J;
1361 }
1362 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
1363 Matrix iDynChain::computeGeoJacobian(const unsigned int iLinkN, const Matrix &Pn, const Matrix &_H0)
1364 {
1365  if(DOF==0)
1366  {
1367  if(verbose)yError("iDynChain: computeGeoJacobian() failed since DOF==0 \n");
1368  return Matrix(0,0);
1369  }
1370  if(iLinkN>=N)
1371  {
1372  if(verbose) yError("iDynChain: computeGeoJacobian() failed due to out of range indexes: from 0 to %d >= %d \n",iLinkN,N);
1373  return Matrix(0,0);
1374  }
1375 
1376  // the jacobian size is linkN+1: eg, index=2, Njoints=0,1,2=3
1377  Matrix J(6, iLinkN+1 );J.zero();
1378  Matrix Z;
1379  Vector w;
1380 
1381  deque<Matrix> intH;
1382  intH.push_back(_H0);
1383  for (unsigned int i=0; i<iLinkN; i++)
1384  intH.push_back(intH[i]*allList[i]->getH(true));
1385 
1386  for (unsigned int i=0; i<iLinkN; i++)
1387  {
1388  unsigned int j=hash[i];
1389  Z=intH[j];
1390  w=cross(Z,2,Pn-Z,3);
1391  J(0,j)=w[0];
1392  J(1,j)=w[1];
1393  J(2,j)=w[2];
1394  J(3,j)=Z(0,2);
1395  J(4,j)=Z(1,2);
1396  J(5,j)=Z(2,2);
1397  }
1398  return J;
1399 }
1400 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
1401 Matrix iDynChain::computeGeoJacobian(const Matrix &Pn)
1402 {
1403  if (DOF==0)
1404  {
1405  if(verbose) yError("iDynChain: computeGeoJacobian() failed since DOF==0 \n");
1406  return Matrix(0,0);
1407  }
1408 
1409  //the jacobian is the same of iKin, but Pn is an input
1410  Matrix J(6,DOF); J.zero();
1411  Matrix Z;
1412  Vector w;
1413 
1414  deque<Matrix> intH;
1415  intH.push_back(H0);
1416  for(unsigned int i=0; i<N; i++)
1417  intH.push_back(intH[i]*allList[i]->getH(true));
1418 
1419  for(unsigned int i=0; i<DOF; i++)
1420  {
1421  unsigned int j=hash[i];
1422  Z=intH[j];
1423  w=cross(Z,2,Pn-Z,3);
1424  J(0,i)=w[0];
1425  J(1,i)=w[1];
1426  J(2,i)=w[2];
1427  J(3,i)=Z(0,2);
1428  J(4,i)=Z(1,2);
1429  J(5,i)=Z(2,2);
1430  }
1431  return J;
1432 }
1433 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
1434 Matrix iDynChain::computeGeoJacobian(const Matrix &Pn, const Matrix &_H0)
1435 {
1436  if (DOF==0)
1437  {
1438  if(verbose) yError("iDynChain: computeGeoJacobian() failed since DOF==0 \n");
1439  return Matrix(0,0);
1440  }
1441 
1442  //the jacobian is the same of iKin, but Pn is an input
1443  Matrix J(6,DOF); J.zero();
1444  Matrix Z;
1445  Vector w;
1446 
1447  deque<Matrix> intH;
1448  intH.push_back(_H0);
1449  for(unsigned int i=0; i<N; i++)
1450  intH.push_back(intH[i]*allList[i]->getH(true));
1451 
1452  for(unsigned int i=0; i<DOF; i++)
1453  {
1454  unsigned int j=hash[i];
1455  Z=intH[j];
1456  w=cross(Z,2,Pn-Z,3);
1457  J(0,i)=w[0];
1458  J(1,i)=w[1];
1459  J(2,i)=w[2];
1460  J(3,i)=Z(0,2);
1461  J(4,i)=Z(1,2);
1462  J(5,i)=Z(2,2);
1463  }
1464  return J;
1465 }
1466 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
1467 Matrix iDynChain::getDenHart(unsigned int i)
1468 {
1469  return allList[i]->getH();
1470 }
1471 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
1472 
1473  //---------------
1474  // jacobians COM
1475  //---------------
1476 
1477 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
1478 Matrix iDynChain::TESTING_computeCOMJacobian(const unsigned int iLink)
1479 {
1480  if (iLink>=N)
1481  {
1482  if(verbose) yError("computeCOMJacobian() failed due to out of range index: %d >= %d \n",iLink, N);
1483  return Matrix(0,0);
1484  }
1485 
1486  //note: all links must be considered! the Jacobian is 6xN, but only the first
1487  //iLink columns are filled.
1488  Matrix J(6,iLink+1);
1489  //Matrix J(6,N); J.zero();
1490  Matrix Pn,Z;
1491  Vector w;
1492 
1493  deque<Matrix> intH;
1494  intH.push_back(H0);
1495 
1496  for (unsigned int j=0; j<=iLink; j++)
1497  intH.push_back(intH[j]*allList[j]->getH(true));
1498 
1499  Pn=intH[iLink+1]*allList[iLink]->getCOM();
1500 
1501  for (unsigned int j=0; j<=iLink; j++)
1502  {
1503  Z=intH[j];
1504  w=cross(Z,2,Pn-Z,3);
1505 
1506  J(0,j)=w[0];
1507  J(1,j)=w[1];
1508  J(2,j)=w[2];
1509  J(3,j)=Z(0,2);
1510  J(4,j)=Z(1,2);
1511  J(5,j)=Z(2,2);
1512  }
1513 
1514  return J;
1515 }
1516 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
1517 Matrix iDynChain::TESTING_computeCOMJacobian(const unsigned int iLink, const Matrix &Pn)
1518 {
1519  if (iLink>=N)
1520  {
1521  if(verbose) yError("computeCOMJacobian() failed due to out of range index: %d >= %d \n", iLink,N);
1522  return Matrix(0,0);
1523  }
1524 
1525  Matrix J(6,iLink+1);
1526  Matrix Z;
1527  Vector w;
1528 
1529  deque<Matrix> intH;
1530  intH.push_back(H0);
1531 
1532  for (unsigned int j=0; j<=iLink; j++)
1533  intH.push_back(intH[j]*allList[j]->getH(true));
1534 
1535  for (unsigned int j=0; j<=iLink; j++)
1536  {
1537  Z=intH[j];
1538  w=cross(Z,2,Pn-Z,3);
1539 
1540  J(0,j)=w[0];
1541  J(1,j)=w[1];
1542  J(2,j)=w[2];
1543  J(3,j)=Z(0,2);
1544  J(4,j)=Z(1,2);
1545  J(5,j)=Z(2,2);
1546  }
1547 
1548  return J;
1549 }
1550 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
1551 Matrix iDynChain::TESTING_computeCOMJacobian(const unsigned int iLink, const Matrix &Pn, const Matrix &_H0)
1552 {
1553  if (iLink>=N)
1554  {
1555  if(verbose) yError("computeCOMJacobian() failed due to out of range index: %d >= %d \n", iLink,N);
1556  return Matrix(0,0);
1557  }
1558 
1559  Matrix J(6,iLink+1);
1560  Matrix Z;
1561  Vector w;
1562 
1563  deque<Matrix> intH;
1564  intH.push_back(_H0);
1565 
1566  for (unsigned int j=0; j<=iLink; j++)
1567  intH.push_back(intH[j]*allList[j]->getH(true));
1568 
1569  for (unsigned int j=0; j<=iLink; j++)
1570  {
1571  Z=intH[j];
1572  w=cross(Z,2,Pn-Z,3);
1573 
1574  J(0,j)=w[0];
1575  J(1,j)=w[1];
1576  J(2,j)=w[2];
1577  J(3,j)=Z(0,2);
1578  J(4,j)=Z(1,2);
1579  J(5,j)=Z(2,2);
1580  }
1581 
1582  return J;
1583 }
1584 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
1585 Matrix iDynChain::getCOM(unsigned int iLink)
1586 {
1587  if (iLink>=N)
1588  {
1589  if(verbose) yError("iDynChain: error, getCOM() failed due to out of range index: %d >= %d \n", iLink,N);
1590  return Matrix(0,0);
1591  }
1592  return allList[iLink]->getCOM();
1593 }
1594 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
1595 Matrix iDynChain::getHCOM(unsigned int iLink)
1596 {
1597  if (iLink>=N)
1598  {
1599  if(verbose) yError("iDynChain: error, getHCOM() failed due to out of range index: %d >= %d \n", iLink,N);
1600  return Matrix(0,0);
1601  }
1602  return getH(iLink,true) * allList[iLink]->getCOM();
1603 }
1604 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
1605 Matrix iDynChain::computeMassMatrix()
1606 {
1607  Matrix M(DOF,DOF); // mass matrix
1608  iKinLink* l;
1609  int hash_i;
1610  setDAng(zeros(DOF)); // set to zero the joint vel
1611  setD2Ang(zeros(DOF)); // set to zero the joint acc
1612 
1613  if(NE==NULL || NE->getMode()!=DYNAMIC)
1614  prepareNewtonEuler(DYNAMIC);
1615  initNewtonEuler(); // init with zero w, dw, ddp, F, Mu
1616  NE->ForwardKinematicFromBase(); // propagate zero kinematics from base
1617 
1618  for(int i=DOF-1; i>=0; i--) // start from the end of the chain
1619  {
1620  l = quickList[hash_dof[i]];
1621  hash_i = hash[i];
1622  l->setD2Ang(1.0); // set i-th accelleration to 1
1623 
1624  // forward kinematics from link i-1 to end
1625  for(unsigned int j=1+hash_i; j<N+1; j++)
1626  NE->neChain[j]->ForwardKinematics(NE->neChain[j-1]);
1627 
1628  // backward wrench from end to link i
1629  for(int j=N; j>=hash_i; j--)
1630  NE->neChain[j]->BackwardWrench(NE->neChain[j+1]);
1631 
1632  // copy the joint torques in the i-th column of M
1633  for(unsigned int j=i; j<DOF; j++)
1634  M(j,i) = M(i,j) = NE->neChain[hash[j]]->getMoment(true)[2];
1635  //same as quickList[hash_dof[j]]->getTorque(), but in this way you do not need to call computeTorque
1636 
1637  l->setD2Ang(0.0); // set i-th accelleration to 0
1638  }
1639 
1640  return M;
1641 }
1642 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
1643 Matrix iDynChain::computeMassMatrix(const Vector& q)
1644 {
1645  setAng(q);
1646  return computeMassMatrix();
1647 }
1648 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
1649 // This is a simpler version of the method, just to understand what the method does.
1650 // The new version is about 2 times faster than this, because it exploits the fact that
1651 // the mass matrix is symmetric (but it is a little harder to understand the code).
1652 //Matrix iDynChain::computeMassMatrix()
1653 //{
1654 // // mass matrix
1655 // Matrix M(DOF,DOF);
1656 // setDAng(zeros(DOF)); // set to zero the joint vel
1657 // setD2Ang(zeros(DOF)); // set to zero the joint acc
1658 // prepareNewtonEuler(DYNAMIC);
1659 // initNewtonEuler();
1660 //
1661 // for(unsigned int i=0; i<DOF; i++)
1662 // {
1663 // quickList[hash_dof[i]]->setD2Ang(1.0); // set i-th accelleration to 1
1664 // computeNewtonEuler(); // gravity and ext wrench are zero
1665 // for(unsigned int j=0; j<DOF; j++)
1666 // M(j,i) = quickList[hash_dof[j]]->getTorque();
1667 // quickList[hash_dof[i]]->setD2Ang(0.0); // set i-th accelleration to 0
1668 // }
1669 //
1670 // return M;
1671 //}
1672 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
1673 Vector iDynChain::computeCcTorques()
1674 {
1675  Vector cc(DOF); // cc torque vector
1676  setD2Ang(zeros(DOF)); // set to zero the joint acc
1677 
1678  if(NE==NULL || NE->getMode()!=DYNAMIC)
1679  prepareNewtonEuler(DYNAMIC);
1680  initNewtonEuler(); // init with zero w, dw, ddp, F, Mu
1681  computeNewtonEuler();
1682  for(unsigned int i=0; i<DOF; i++)
1683  cc(i) = quickList[hash_dof[i]]->getTorque();
1684 
1685  return cc;
1686 }
1687 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
1688 Vector iDynChain::computeCcTorques(const Vector& q, const Vector& dq)
1689 {
1690  setAng(q);
1691  setDAng(dq);
1692  return computeCcTorques();
1693 }
1694 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
1695 Vector iDynChain::computeGravityTorques(const Vector& ddp0)
1696 {
1697  Vector g(DOF), zero3(3, 0.0);
1698  if(NE==NULL || NE->getMode()!=STATIC)
1699  prepareNewtonEuler(STATIC);
1700  initNewtonEuler(zero3, zero3, ddp0, zero3, zero3); // init with zero w, dw, F, Mu
1701  computeNewtonEuler();
1702  for(unsigned int i=0; i<DOF; i++)
1703  g(i) = quickList[hash_dof[i]]->getTorque();
1704  return g;
1705 }
1706 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
1707 Vector iDynChain::computeGravityTorques(const Vector& ddp0, const Vector& q)
1708 {
1709  setAng(q);
1710  return computeGravityTorques(ddp0);
1711 }
1712 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
1713 Vector iDynChain::computeCcGravityTorques(const Vector& ddp0)
1714 {
1715  Vector ccg(DOF), zero3(3, 0.0);
1716  setD2Ang(zeros(DOF)); // set to zero the joint acc
1717 
1718  if(NE==NULL || NE->getMode()!=DYNAMIC)
1719  prepareNewtonEuler(DYNAMIC);
1720  initNewtonEuler(zero3, zero3, ddp0, zero3, zero3); // init with zero w, dw, F, Mu
1721  computeNewtonEuler();
1722  for(unsigned int i=0; i<DOF; i++)
1723  ccg(i) = quickList[hash_dof[i]]->getTorque();
1724  return ccg;
1725 }
1726 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
1727 Vector iDynChain::computeCcGravityTorques(const Vector& ddp0, const Vector& q, const Vector& dq)
1728 {
1729  setAng(q);
1730  setDAng(dq);
1731  return computeCcGravityTorques(ddp0);
1732 }
1733 
1734 
1735 //================================
1736 //
1737 // I DYN LIMB
1738 //
1739 //================================
1740 
1741 
1742 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
1743 iDynLimb::iDynLimb()
1744 {
1745  allocate("right");
1746 }
1747 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
1748 iDynLimb::iDynLimb(const string &_type)
1749 {
1750  allocate(_type);
1751 }
1752 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
1753 iDynLimb::iDynLimb(const iDynLimb &limb)
1754 {
1755  clone(limb);
1756 }
1757 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
1758 iDynLimb::iDynLimb(const Property &option)
1759 {
1760  fromLinksProperties(option);
1761 }
1762 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
1763 void iDynLimb::pushLink(iDynLink *pl)
1764 {
1765  linkList.push_back(pl);
1766  pushLink(*pl);
1767 }
1768 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
1769 bool iDynLimb::fromLinksProperties(const Property &option)
1770 {
1771  dispose();
1772  int i,j;
1773  Matrix I(3,3);
1774  Matrix HC(4,4);
1775 
1776  // type: left/right
1777  type=option.check("type",Value("right")).asString().c_str();
1778  if(type!="right" && type!="left")
1779  {
1780  yError("Error: invalid handedness type specified! \n");
1781  return false;
1782  }
1783 
1784  // H0 matrix
1785  if(Bottle *bH0=option.find("H0").asList())
1786  {
1787  i=0;
1788  j=0;
1789  H0.zero();
1790  for(int cnt=0; (cnt<bH0->size()) && (cnt<H0.rows()*H0.cols()); cnt++)
1791  {
1792  H0(i,j)=bH0->get(cnt).asFloat64();
1793  if(++j>=H0.cols())
1794  {
1795  i++;
1796  j=0;
1797  }
1798  }
1799  }
1800 
1801  //number of links
1802  int numLinks=option.check("numLinks",Value(0)).asInt32();
1803  if(numLinks==0)
1804  {
1805  yError("Error: invalid number of links (0) specified! \n");
1806  type="right";
1807  H0.eye();
1808  return false;
1809  }
1810 
1811  for(int iLink=0; iLink<numLinks; iLink++)
1812  {
1813  char link[255];
1814  sprintf(link,"link_%d",iLink);
1815  //look for link_i into the property parameters
1816  Bottle &bLink=option.findGroup(link);
1817  if(bLink.isNull())
1818  {
1819  yError("Error: link %d is missing! \n",iLink);
1820  type="right";
1821  H0.eye();
1822  dispose();
1823  return false;
1824  }
1825 
1826  //kinematics parameters
1827  double A=bLink.check("A",Value(0.0)).asFloat64();
1828  double D=bLink.check("D",Value(0.0)).asFloat64();
1829  double alpha=CTRL_DEG2RAD*bLink.check("alpha",Value(0.0)).asFloat64();
1830  double offset=CTRL_DEG2RAD*bLink.check("offset",Value(0.0)).asFloat64();
1831  double min=CTRL_DEG2RAD*bLink.check("min",Value(0.0)).asFloat64();
1832  double max=CTRL_DEG2RAD*bLink.check("max",Value(0.0)).asFloat64();
1833 
1834  //dynamic parameters
1835  //mass
1836  double mass=bLink.check("mass",Value(0.0)).asFloat64();
1837  //inertia
1838  if(Bottle *bI=option.find("Inertia").asList())
1839  {
1840  i=0; j=0;
1841  I.zero();
1842  for(int cnt=0; (cnt<bI->size()) && (cnt<I.rows()*I.cols()); cnt++)
1843  {
1844  I(i,j)=bI->get(cnt).asFloat64();
1845  if(++j>=I.cols())
1846  {
1847  i++;
1848  j=0;
1849  }
1850  }
1851  }
1852  //HC
1853  if(Bottle *bHC=option.find("H_COM").asList())
1854  {
1855  i=0; j=0;
1856  HC.zero();
1857  for(int cnt=0; (cnt<bHC->size()) && (cnt<HC.rows()*HC.cols()); cnt++)
1858  {
1859  HC(i,j)=bHC->get(cnt).asFloat64();
1860  if(++j>=HC.cols())
1861  {
1862  i++;
1863  j=0;
1864  }
1865  }
1866  }
1867 
1868  //create iDynLink from parameters
1869  pushLink(new iDynLink(mass,HC,I,A,D,alpha,offset,min,max));
1870 
1871  if(bLink.check("blocked"))
1872  blockLink(iLink,CTRL_DEG2RAD*bLink.find("blocked").asFloat64());
1873  }
1874 
1875  return configured=true;
1876 }
1877 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
1878 iDynLimb &iDynLimb::operator=(const iDynLimb &limb)
1879 {
1880  dispose();
1881  clone(limb);
1882 
1883  return *this;
1884 }
1885 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
1886 iDynLimb::~iDynLimb()
1887 {
1888  dispose();
1889 }
1890 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
1891 void iDynLimb::allocate(const string &_type)
1892 {
1893  type=_type;
1894 
1895  if(type!="right" && type!="left")
1896  type="right";
1897 
1898  configured=true;
1899 }
1900 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
1901 void iDynLimb::clone(const iDynLimb &limb)
1902 {
1903  type=limb.type;
1904  verbose = limb.verbose;
1905  N = limb.N;
1906  DOF = limb.DOF;
1907 
1908  H0=limb.H0;
1909  curr_q = limb.curr_q;
1910  curr_dq = limb.curr_dq;
1911  curr_ddq = limb.curr_ddq;
1912 
1913  //now copy all lists
1914  for(unsigned int i=0; i<limb.getN(); i++)
1915  {
1916  pushLink(new iDynLink(*(limb.linkList[i])));
1917 
1918  //push into quick list and hash lists (see iKinChain::buildChain())
1919  quickList.push_back(allList[i]);
1920  hash_dof.push_back(quickList.size()-1);
1921  hash.push_back(i);
1922  }
1923  configured=limb.configured;
1924 }
1925 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
1926 void iDynLimb::dispose()
1927 {
1928  for(unsigned int i=0; i<linkList.size(); i++)
1929  if(linkList[i])
1930  delete linkList[i];
1931 
1932  linkList.clear();
1933  iDynChain::dispose();
1934 
1935  configured=false;
1936 }
1937 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
1938 
1939 
1940 
1941 
1942 
1943 //======================================
1944 //
1945 // ICUB ARM DYN
1946 //
1947 //======================================
1948 
1949 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
1950 iCubArmDyn::iCubArmDyn()
1951 {
1952  allocate("right");
1953  setIterMode(KINFWD_WREBWD);
1954 }
1955 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
1956 iCubArmDyn::iCubArmDyn(const string &_type, const ChainComputationMode _mode)
1957 {
1958  allocate(_type);
1959  setIterMode(_mode);
1960 }
1961 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
1962 iCubArmDyn::iCubArmDyn(const iCubArmDyn &arm)
1963 {
1964  clone(arm);
1965 }
1966 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
1967 void iCubArmDyn::allocate(const string &_type)
1968 {
1969  iDynLimb::allocate(_type);
1970 
1971  Matrix H0(4,4);
1972  H0.zero();
1973  H0(0,1)=-1.0;
1974  H0(1,2)=-1.0;
1975  H0(2,0)=1.0;
1976  H0(3,3)=1.0;
1977  setH0(H0);
1978 
1979  if (getType()=="right")
1980  {
1981  // iDynLink(mass, rC (3x1), I(6x1), A, D, alfa, offset, min, max);
1982  pushLink(new iDynLink(0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0.032, 0.0, M_PI/2.0, 0.0, -22.0*CTRL_DEG2RAD, 84.0*CTRL_DEG2RAD));
1983  pushLink(new iDynLink(0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0.0, 0.0, M_PI/2.0, -M_PI/2.0, -39.0*CTRL_DEG2RAD, 39.0*CTRL_DEG2RAD));
1984  pushLink(new iDynLink(0, 0, 0, 0, 0, 0, 0, 0, 0, 0, -0.0233647, -0.1433, M_PI/2.0, -105.0*CTRL_DEG2RAD, -59.0*CTRL_DEG2RAD, 59.0*CTRL_DEG2RAD));
1985  pushLink(new iDynLink(0.189, 0.005e-3, 18.7e-3, 1.19e-3, 123.0e-6, 0.021e-6, -0.001e-6, 24.4e-6, 4.22e-6, 113.0e-6, 0.0, -0.10774, M_PI/2.0, -M_PI/2.0, -95.5*CTRL_DEG2RAD, 5.0*CTRL_DEG2RAD));
1986  pushLink(new iDynLink(0.179, -0.094e-3, -6.27e-3, -16.6e-3, 137.0e-6, -0.453e-06, 0.203e-06, 83.0e-6, 20.7e-6, 99.3e-6, 0.0, 0.0, -M_PI/2.0, -M_PI/2.0, 0.0, 160.8*CTRL_DEG2RAD));
1987  pushLink(new iDynLink(0.884, 1.79e-3, -62.9e-3, 0.064e-03, 743.0e-6, 63.9e-6, 0.851e-06, 336.0e-6, -3.61e-6, 735.0e-6, -0.015, -0.15228, -M_PI/2.0, -105.0*CTRL_DEG2RAD, -37.0*CTRL_DEG2RAD, 90.0*CTRL_DEG2RAD));
1988  pushLink(new iDynLink(0.074, -13.7e-3, -3.71e-3, 1.05e-3, 28.4e-6, -0.502e-6, -0.399e-6, 9.24e-6, -0.371e-6, 29.9e-6, 0.015, 0.0, M_PI/2.0, 0.0, 5.5*CTRL_DEG2RAD, 106.0*CTRL_DEG2RAD));
1989  pushLink(new iDynLink(0.525, -0.347e-3, 71.3e-3, -4.76e-3, 766.0e-6, 5.66e-6, 1.40e-6, 164.0e-6, 18.2e-6, 699.0e-6, 0.0, -0.1373, M_PI/2.0, -M_PI/2.0, -90.0*CTRL_DEG2RAD, 90.0*CTRL_DEG2RAD));
1990  pushLink(new iDynLink( 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0.0, 0.0, M_PI/2.0, M_PI/2.0, -90.0*CTRL_DEG2RAD, 0.0*CTRL_DEG2RAD));
1991  pushLink(new iDynLink(0.213, 7.73e-3, -8.05e-3, -9.00e-3, 154.0e-6, 12.6e-6, -6.08e-6, 250.0e-6, 17.6e-6, 378.0e-6, 0.0625, 0.016, 0.0, M_PI, -20.0*CTRL_DEG2RAD, 40.0*CTRL_DEG2RAD));
1992  }
1993  else
1994  {
1995  pushLink(new iDynLink(0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0.032, 0.0, M_PI/2.0, 0.0, -22.0*CTRL_DEG2RAD, 84.0*CTRL_DEG2RAD));
1996  pushLink(new iDynLink(0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0.0, 0.0, M_PI/2.0, -M_PI/2.0, -39.0*CTRL_DEG2RAD, 39.0*CTRL_DEG2RAD));
1997  pushLink(new iDynLink(0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0.0233647, -0.1433, -M_PI/2.0, 105.0*CTRL_DEG2RAD, -59.0*CTRL_DEG2RAD, 59.0*CTRL_DEG2RAD));
1998  pushLink(new iDynLink(0.13, -0.004e-3, 14.915e-3, -0.019e-3, 54.421e-6, 0.009e-6, 0.0e-6, 9.331e-6, -0.017e-6, 54.862e-6, 0.0, 0.10774, -M_PI/2.0, M_PI/2.0, -95.5*CTRL_DEG2RAD, 5.0*CTRL_DEG2RAD));
1999  pushLink(new iDynLink(0.178, 0.097e-3, -6.271e-3, 16.622e-3, 137.2e-6, 0.466e-6, 0.365e-6, 82.927e-6, -20.524e-6, 99.274e-6, 0.0, 0.0, M_PI/2.0, -M_PI/2.0, 0.0, 160.8*CTRL_DEG2RAD));
2000  pushLink(new iDynLink(0.894, -1.769e-3, 63.302e-3, -0.084e-3, 748.531e-6, 63.340e-6, -0.903e-6, 338.109e-6, -4.031e-6, 741.022e-6, 0.015, 0.15228, -M_PI/2.0, 75.0*CTRL_DEG2RAD, -37.0*CTRL_DEG2RAD, 90.0*CTRL_DEG2RAD));
2001  pushLink(new iDynLink(0.074, 13.718e-3, 3.712e-3, -1.046e-3, 28.389e-6, -0.515e-6, -0.408e-6, 9.244e-6, -0.371e-6, 29.968e-6, -0.015, 0.0, M_PI/2.0, 0.0, 5.5*CTRL_DEG2RAD, 106.0*CTRL_DEG2RAD));
2002  pushLink(new iDynLink(0.525, 0.264e-3, -71.327e-3, 4.672e-3, 765.393e-6, 4.337e-6, 0.239e-6, 164.578e-6, 19.381e-6, 698.060e-6, 0.0, 0.1373, M_PI/2.0, -M_PI/2.0, -90.0*CTRL_DEG2RAD, 90.0*CTRL_DEG2RAD));
2003  pushLink(new iDynLink( 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0.0, 0.0, M_PI/2.0, M_PI/2.0, -90.0*CTRL_DEG2RAD, 0.0*CTRL_DEG2RAD));
2004  pushLink(new iDynLink(0.214, 7.851e-3, -8.319e-3, 9.284e-3, 157.143e-6, 12.780e-6, 4.823e-6, 247.995e-6, -18.188e-6, 380.535e-6, 0.0625, -0.016, 0.0, 0.0, -20.0*CTRL_DEG2RAD, 40.0*CTRL_DEG2RAD));
2005  }
2006 
2007  blockLink(0,0.0);
2008  blockLink(1,0.0);
2009  blockLink(2,0.0);
2010 
2011 #ifdef NO_JOINT_COSTRAINTS
2012  this->setAllConstraints(false);
2013 #endif
2014 }
2015 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
2016 bool iCubArmDyn::alignJointsBounds(const deque<IControlLimits*> &lim)
2017 {
2018  if (lim.size()<2)
2019  return false;
2020 
2021  IControlLimits &limTorso=*lim[0];
2022  IControlLimits &limArm =*lim[1];
2023 
2024  unsigned int iTorso;
2025  unsigned int iArm;
2026  double min, max;
2027 
2028  for (iTorso=0; iTorso<3; iTorso++)
2029  {
2030  if (!limTorso.getLimits(iTorso,&min,&max))
2031  return false;
2032 
2033  (*this)[2-iTorso].setMin(CTRL_DEG2RAD*min);
2034  (*this)[2-iTorso].setMax(CTRL_DEG2RAD*max);
2035  }
2036 
2037  for (iArm=0; iArm<getN()-iTorso; iArm++)
2038  {
2039  if (!limArm.getLimits(iArm,&min,&max))
2040  return false;
2041 
2042  (*this)[iTorso+iArm].setMin(CTRL_DEG2RAD*min);
2043  (*this)[iTorso+iArm].setMax(CTRL_DEG2RAD*max);
2044  }
2045 
2046  return true;
2047 }
2048 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
2049 
2050 
2051 
2052 
2053 //======================================
2054 //
2055 // ICUB ARM NO TORSO DYN
2056 //
2057 //======================================
2058 
2059 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
2060 iCubArmNoTorsoDyn::iCubArmNoTorsoDyn()
2061 {
2062  allocate("right");
2063  setIterMode(KINFWD_WREBWD);
2064 }
2065 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
2066 iCubArmNoTorsoDyn::iCubArmNoTorsoDyn(const string &_type, const ChainComputationMode _mode)
2067 {
2068  allocate(_type);
2069  setIterMode(_mode);
2070 }
2071 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
2072 iCubArmNoTorsoDyn::iCubArmNoTorsoDyn(const iCubArmNoTorsoDyn &arm)
2073 {
2074  clone(arm);
2075 }
2076 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
2077 void iCubArmNoTorsoDyn::allocate(const string &_type)
2078 {
2079  iDynLimb::allocate(_type);
2080 
2081  Matrix H0(4,4);
2082  H0.eye();
2083  setH0(H0);
2084 
2085  if (getType()=="right")
2086  {
2087  //note: the D value in joint 0 is different from the corresponding one in iCubArmDyn (and iKin::iCubArm)
2088  // in ikin: -0.10774 here: 0.0
2089  // see the RBT matrix of the right arm connected to the UpperTorso or UpperBody nodes
2090  pushLink(new iDynLink(0.189*mra, 0.005e-3, 18.7e-3, 1.19e-3, 123.0e-6, 0.021e-6, -0.001e-6, 24.4e-6, 4.22e-6, 113.0e-6, 0.0, -0.0, M_PI/2.0, -M_PI/2.0, -95.5*CTRL_DEG2RAD, 5.0*CTRL_DEG2RAD));
2091  pushLink(new iDynLink(0.179*mra, -0.094e-3, -6.27e-3, -16.6e-3, 137.0e-6, -0.453e-06, 0.203e-06, 83.0e-6, 20.7e-6, 99.3e-6, 0.0, 0.0, -M_PI/2.0, -M_PI/2.0, 0.0, 160.8*CTRL_DEG2RAD));
2092  pushLink(new iDynLink(0.884*mra, 1.79e-3, -62.9e-3, 0.064e-03, 743.0e-6, 63.9e-6, 0.851e-06, 336.0e-6, -3.61e-6, 735.0e-6, -0.015, -0.15228, -M_PI/2.0, -105.0*CTRL_DEG2RAD, -37.0*CTRL_DEG2RAD, 90.0*CTRL_DEG2RAD));
2093  pushLink(new iDynLink(0.074*mra, -13.7e-3, -3.71e-3, 1.05e-3, 28.4e-6, -0.502e-6, -0.399e-6, 9.24e-6, -0.371e-6, 29.9e-6, 0.015, 0.0, M_PI/2.0, 0.0, 5.5*CTRL_DEG2RAD, 106.0*CTRL_DEG2RAD));
2094  pushLink(new iDynLink(0.525*mra, -0.347e-3, 71.3e-3, -4.76e-3, 766.0e-6, 5.66e-6, 1.40e-6, 164.0e-6, 18.2e-6, 699.0e-6, 0.0, -0.1373, M_PI/2.0, -M_PI/2.0, -90.0*CTRL_DEG2RAD, 90.0*CTRL_DEG2RAD));
2095  pushLink(new iDynLink( 0*mra, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0.0, 0.0, M_PI/2.0, M_PI/2.0, -90.0*CTRL_DEG2RAD, 0.0*CTRL_DEG2RAD));
2096  pushLink(new iDynLink(0.213*mra, 7.73e-3, -8.05e-3, -9.00e-3, 154.0e-6, 12.6e-6, -6.08e-6, 250.0e-6, 17.6e-6, 378.0e-6, 0.0625, 0.016, 0.0, M_PI, -20.0*CTRL_DEG2RAD, 40.0*CTRL_DEG2RAD));
2097  }
2098  else
2099  {
2100  //note: the D value in joint 0 is different from the corresponding one in iCubArmDyn (and iKin::iCubArm)
2101  // in ikin: +0.10774 here: 0.0
2102  // see the RBT matrix of the right arm connected to the UpperTorso or UpperBody nodes
2103  pushLink(new iDynLink(0.189*mla, -0.005e-3, 18.7e-3, -1.19e-3, 54.421e-6, 0.009e-6, 0.0e-6, 9.331e-6, -0.017e-6, 54.862e-6, 0.0, 0.0, -M_PI/2.0, M_PI/2.0, -95.5*CTRL_DEG2RAD, 5.0*CTRL_DEG2RAD));
2104  pushLink(new iDynLink(0.179*mla, 0.094e-3, -6.27e-3, 16.6e-3, 137.2e-6, 0.466e-6, 0.365e-6, 82.927e-6, -20.524e-6, 99.274e-6, 0.0, 0.0, M_PI/2.0, -M_PI/2.0, 0.0, 160.8*CTRL_DEG2RAD));
2105  pushLink(new iDynLink(0.884*mla, -1.79e-3, 62.9e-3, -0.064e-3, 748.531e-6, 63.340e-6, -0.903e-6, 338.109e-6, -4.031e-6, 741.022e-6, 0.015, 0.15228, -M_PI/2.0, 75.0*CTRL_DEG2RAD, -37.0*CTRL_DEG2RAD, 90.0*CTRL_DEG2RAD));
2106  pushLink(new iDynLink(0.074*mla, 13.7e-3, 3.71e-3, -1.05e-3, 28.389e-6, -0.515e-6, -0.408e-6, 9.244e-6, -0.371e-6, 29.968e-6, -0.015, 0.0, M_PI/2.0, 0.0, 5.5*CTRL_DEG2RAD, 106.0*CTRL_DEG2RAD));
2107  pushLink(new iDynLink(0.525*mla, 0.347e-3, -71.3e-3, 4.76e-3, 765.393e-6, 4.337e-6, 0.239e-6, 164.578e-6, 19.381e-6, 698.060e-6, 0.0, 0.1373, M_PI/2.0, -M_PI/2.0, -90.0*CTRL_DEG2RAD, 90.0*CTRL_DEG2RAD));
2108  pushLink(new iDynLink( 0*mla, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0.0, 0.0, M_PI/2.0, M_PI/2.0, -90.0*CTRL_DEG2RAD, 0.0*CTRL_DEG2RAD));
2109  pushLink(new iDynLink(0.213*mla, 7.73e-3, -8.05e-3, 9.00e-3, 157.143e-6, 12.780e-6, 4.823e-6, 247.995e-6, -18.188e-6, 380.535e-6, 0.0625, -0.016, 0.0, 0.0, -20.0*CTRL_DEG2RAD, 40.0*CTRL_DEG2RAD));
2110  }
2111 
2112 #ifdef NO_JOINT_COSTRAINTS
2113  this->setAllConstraints(false);
2114 #endif
2115 }
2116 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
2117 bool iCubArmNoTorsoDyn::alignJointsBounds(const deque<IControlLimits*> &lim)
2118 {
2119  if (lim.size()<1)
2120  return false;
2121 
2122  IControlLimits &limArm =*lim[0];
2123 
2124  unsigned int iArm;
2125  double min, max;
2126 
2127  for (iArm=0; iArm<getN(); iArm++)
2128  {
2129  if (!limArm.getLimits(iArm,&min,&max))
2130  return false;
2131 
2132  (*this)[iArm].setMin(CTRL_DEG2RAD*min);
2133  (*this)[iArm].setMax(CTRL_DEG2RAD*max);
2134  }
2135 
2136  return true;
2137 }
2138 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
2139 
2140 
2141 
2142 //======================================
2143 //
2144 // ICUB TORSO DYN
2145 //
2146 //======================================
2147 
2148 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
2149 iCubTorsoDyn::iCubTorsoDyn()
2150 {
2151  allocate("lower");
2152  setIterMode(KINBWD_WREBWD);
2153 }
2154 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
2155 iCubTorsoDyn::iCubTorsoDyn(const string &_type, const ChainComputationMode _mode)
2156 {
2157  allocate(_type);
2158  setIterMode(_mode);
2159 }
2160 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
2161 iCubTorsoDyn::iCubTorsoDyn(const iCubTorsoDyn &torso)
2162 {
2163  clone(torso);
2164 }
2165 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
2166 void iCubTorsoDyn::allocate(const string &_type)
2167 {
2168  type = _type;
2169 
2170  // note: H0 here is an identity, whereas in iCubArm and iCubArmDyn is different (0 -1 00; 00 -1 0; 1 000; 0001 by row)
2171  // the reason is that the torso limb is cursed, and its H0 is different if we consider it a standalone limb attached
2172  // to upper body or lower torso nodes.
2173  // check the RBT matrix linking torso to each node, and set the H0 properly before performing computations involving it
2174  // like the Jacobians..
2175  Matrix H0(4,4);
2176  H0.eye();
2177  setH0(H0);
2178 
2179  // iDynLink( mass, rC (3x1), I(6x1), A, D, alfa, offset, min, max);
2180 // These parameters are still under debug!
2181 //#define TORSO_NO_WEIGHT
2182 #ifdef TORSO_NO_WEIGHT
2183  pushLink(new iDynLink(0.00e-3, 0.000e-3, 0.000e-3, 0.000e-3, 0.000e-6, 0.000e-6, 0.000e-6, 0.000e-6, 0.000e-6, 0.000e-6, 32.0e-3, 0, M_PI/2.0, 0.0, -100.0*CTRL_DEG2RAD, 100.0*CTRL_DEG2RAD));
2184  pushLink(new iDynLink(0.00e-3, 0.000e-3, 0.000e-3, 0.000e-3, 0.000e-6, 0.000e-6, 0.000e-6, 0.000e-6, 0.000e-6, 0.000e-6, 0, -5.5e-3, M_PI/2.0, -M_PI/2.0, -100.0*CTRL_DEG2RAD, 100.0*CTRL_DEG2RAD));
2185  pushLink(new iDynLink(0.00e-3, 0.000e-3, 0.000e-3, 0.000e-3, 0.000e-6, 0.000e-6, 0.000e-6, 0.000e-6, 0.000e-6, 0.000e-6, 2.31e-3, -193.3e-3, -M_PI/2.0, -M_PI/2.0, -100.0*CTRL_DEG2RAD, 100.0*CTRL_DEG2RAD));
2186 #else
2187 // This version of the parameters has been taken from the CAD...
2188 // pushLink(new iDynLink(7.79e-1, 3.120e-2, 6.300e-4, -9.758e-7, 4.544e-4, -4.263e-5, -3.889e-8, 1.141e-3, 0.000e-0, 1.236e-3, 32.0e-3, 0, M_PI/2.0, 0.0, -22.0*CTRL_DEG2RAD, 84.0*CTRL_DEG2RAD));
2189 // pushLink(new iDynLink(5.77e-1, 4.265e-2, +4.296e-5, -1.360e-3, 5.308e-4, -1.923e-6, 5.095e-5, 2.031e-3, -3.849e-7, 1.803e-3, 0, -5.5e-3, M_PI/2.0, -M_PI/2.0, -39.0*CTRL_DEG2RAD, 39.0*CTRL_DEG2RAD));
2190 // pushLink(new iDynLink(4.81e+0, -8.102e-5, 7.905e-3, -1.183e-1, 7.472e-2, -3.600e-6, -4.705e-5, 8.145e-2, 4.567e-3, 1.306e-2, 2.31e-3, -193.3e-3, -M_PI/2.0, -M_PI/2.0, -59.0*CTRL_DEG2RAD, 59.0*CTRL_DEG2RAD));
2191 // ...but they have still to be verified. In the meanwhile use this for debug:
2192  pushLink(new iDynLink(0, 3.120e-2, 0, -9.758e-7, 4.544e-4, -4.263e-5, -3.889e-8, 1.141e-3, 0.000e-0, 1.236e-3, 32.0e-3, 0, M_PI/2.0, 0.0, -100.0*CTRL_DEG2RAD, 100.0*CTRL_DEG2RAD));
2193  pushLink(new iDynLink(0, 0, +4.296e-5, -1.360e-3, 5.308e-4, -1.923e-6, 5.095e-5, 2.031e-3, -3.849e-7, 1.803e-3, 0, -5.5e-3, M_PI/2.0, -M_PI/2.0, -100.0*CTRL_DEG2RAD, 100.0*CTRL_DEG2RAD));
2194  pushLink(new iDynLink(4.81e+0, -8.102e-5, -1.183e-1, 0, 7.472e-2, -3.600e-6, -4.705e-5, 8.145e-2, 4.567e-3, 1.306e-2, 2.31e-3, -193.3e-3, -M_PI/2.0, -M_PI/2.0, -100.0*CTRL_DEG2RAD, 100.0*CTRL_DEG2RAD));
2195 #endif
2196 
2197 #ifdef NO_JOINT_COSTRAINTS
2198  this->setAllConstraints(false);
2199 #endif
2200 }
2201 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
2202 bool iCubTorsoDyn::alignJointsBounds(const deque<IControlLimits*> &lim)
2203 {
2204  if (lim.size()<1)
2205  return false;
2206 
2207  IControlLimits &limTorso=*lim[0];
2208 
2209  unsigned int iTorso;
2210  double min, max;
2211 
2212  for (iTorso=0; iTorso<3; iTorso++)
2213  {
2214  if (!limTorso.getLimits(iTorso,&min,&max))
2215  return false;
2216 
2217  (*this)[2-iTorso].setMin(CTRL_DEG2RAD*min);
2218  (*this)[2-iTorso].setMax(CTRL_DEG2RAD*max);
2219  }
2220 
2221  return true;
2222 }
2223 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
2224 
2225 
2226 
2227 
2228 
2229 
2230 //======================================
2231 //
2232 // ICUB LEG DYN
2233 //
2234 //======================================
2235 
2236 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
2237 
2238 iCubLegDyn::iCubLegDyn()
2239 {
2240  allocate("right");
2241  setIterMode(KINFWD_WREBWD);
2242 }
2243 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
2244 iCubLegDyn::iCubLegDyn(const string &_type,const ChainComputationMode _mode)
2245 {
2246  allocate(_type);
2247  setIterMode(_mode);
2248 }
2249 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
2250 iCubLegDyn::iCubLegDyn(const iCubLegDyn &leg)
2251 {
2252  clone(leg);
2253 }
2254 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
2255 void iCubLegDyn::allocate(const string &_type)
2256 {
2257  iDynLimb::allocate(_type);
2258 
2259  Matrix H0(4,4);
2260  H0.eye();
2261  setH0(H0);
2262 
2263 #ifdef LEGS_NO_WEIGHT
2264  if(getType()=="right")
2265  {
2266  //create iDynLink from parameters calling
2267  //pushLink(new iDynLink(mass,HC,I,A,D,alfa,offset,min,max));
2268  // m rcx rcy rcZ I1 I2 I3 I4 I5 I6 A D alpha offset
2269  pushLink(new iDynLink(0, 0, -0.0782, 0, 0, 0, 0, 0, 0, 0, 0.0, 0.0, M_PI/2.0, M_PI/2.0, -44.0*CTRL_DEG2RAD, 132.0*CTRL_DEG2RAD));
2270  pushLink(new iDynLink(0, 0, 0, 0.03045, 0, 0, 0, 0, 0, 0, 0.0, 0.0, M_PI/2.0, M_PI/2.0, -17.0*CTRL_DEG2RAD, 119.0*CTRL_DEG2RAD));
2271  pushLink(new iDynLink(0, 0.00144, 0.06417, 0.00039, 0, 0, 0, 0, 0, 0, 0.0, 0.2236, -M_PI/2.0, -M_PI/2.0, -79.0*CTRL_DEG2RAD, 79.0*CTRL_DEG2RAD));
2272  pushLink(new iDynLink(0, 0.1059, 0.00182, -0.00211, 0, 0, 0, 0, 0, 0, -0.213, 0.0, M_PI, M_PI/2.0, -125.0*CTRL_DEG2RAD, 23.0*CTRL_DEG2RAD));
2273  pushLink(new iDynLink(0, -0.0054, 0.00163, -0.0172, 0, 0, 0, 0, 0, 0, 0.0, 0.0, M_PI/2.0, 0.0, -42.0*CTRL_DEG2RAD, 21.0*CTRL_DEG2RAD));
2274  pushLink(new iDynLink(0, 0, 0, 0, 0, 0, 0, 0, 0, 0, -0.041, 0.0, M_PI, 0.0, -24.0*CTRL_DEG2RAD, 24.0*CTRL_DEG2RAD));
2275 
2276  }
2277  else
2278  {
2279  pushLink(new iDynLink(0, 0, -0.0782, 0, 0, 0, 0, 0, 0, 0, 0.0, 0.0, -M_PI/2.0, M_PI/2.0, -44.0*CTRL_DEG2RAD, 132.0*CTRL_DEG2RAD));
2280  pushLink(new iDynLink(0, 0, 0, -0.03045, 0, 0, 0, 0, 0, 0, 0.0, 0.0, -M_PI/2.0, M_PI/2.0, -17.0*CTRL_DEG2RAD, 119.0*CTRL_DEG2RAD));
2281  pushLink(new iDynLink(0, 0.00144, 0.06417, -0.00039, 0, 0, 0, 0, 0, 0, 0.0, -0.2236, M_PI/2.0, -M_PI/2.0, -79.0*CTRL_DEG2RAD, 79.0*CTRL_DEG2RAD));
2282  pushLink(new iDynLink(0, 0.1059, 0.00182, 0.00211, 0, 0, 0, 0, 0, 0, -0.213, 0.0, M_PI, M_PI/2.0, -125.0*CTRL_DEG2RAD, 23.0*CTRL_DEG2RAD));
2283  pushLink(new iDynLink(0, -0.0054, 0.00163, 0.0172, 0, 0, 0, 0, 0, 0, 0.0, 0.0, -M_PI/2.0, 0.0, -42.0*CTRL_DEG2RAD, 21.0*CTRL_DEG2RAD));
2284  pushLink(new iDynLink(0, 0, 0, 0, 0, 0, 0, 0, 0, 0, -0.041, 0.0, 0.0, 0.0, -24.0*CTRL_DEG2RAD, 24.0*CTRL_DEG2RAD));
2285  }
2286 #else
2287  if(getType()=="right")
2288  {
2289  //create iDynLink from parameters calling
2290  //pushLink(new iDynLink(mass,HC,I,A,D,alfa,offset,min,max));
2291 
2292  pushLink(new iDynLink(0.754, 0, -0.0782, 0, 0, 0, 0, 0, 0, 0, 0.0, 0.0, M_PI/2.0, M_PI/2.0, -44.0*CTRL_DEG2RAD, 132.0*CTRL_DEG2RAD));
2293  pushLink(new iDynLink(0.526, 0, 0, 0.03045, 0, 0, 0, 0, 0, 0, 0.0, 0.0, M_PI/2.0, M_PI/2.0, -17.0*CTRL_DEG2RAD, 119.0*CTRL_DEG2RAD));
2294  pushLink(new iDynLink(2.175, 0.00144, 0.06417, 0.00039, 0, 0, 0, 0, 0, 0, 0.0, 0.2236, -M_PI/2.0, -M_PI/2.0, -79.0*CTRL_DEG2RAD, 79.0*CTRL_DEG2RAD));
2295  pushLink(new iDynLink(1.264, 0.1059, 0.00182, -0.00211, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, -0.213, 0.0, M_PI, M_PI/2.0, -125.0*CTRL_DEG2RAD, 23.0*CTRL_DEG2RAD));
2296  pushLink(new iDynLink(0.746, -0.0054, 0.00163, -0.0172, 0, 0, 0, 0, 0, 0, 0.0, 0.0, M_PI/2.0, 0.0, -42.0*CTRL_DEG2RAD, 21.0*CTRL_DEG2RAD));
2297  pushLink(new iDynLink(0.010, 0, 0, 0, 0, 0, 0, 0, 0, 0, -0.041, 0.0, M_PI, 0.0, -24.0*CTRL_DEG2RAD, 24.0*CTRL_DEG2RAD));
2298 
2299  }
2300  else
2301  {
2302  pushLink(new iDynLink(0.754, 0, -0.0782, 0, 471.076e-6, 2.059e-6, 1.451e-6, 346.478e-6, 1.545e-6, 510.315e-6, 0.0, 0.0, -M_PI/2.0, M_PI/2.0, -44.0*CTRL_DEG2RAD, 132.0*CTRL_DEG2RAD));
2303  pushLink(new iDynLink(0.526, 0, 0, -0.03045, 738.0487e-6, -0.074e-6, -0.062e-6, 561.583e-6, 10.835e-6, 294.119e-6, 0.0, 0.0, -M_PI/2.0, M_PI/2.0, -17.0*CTRL_DEG2RAD, 119.0*CTRL_DEG2RAD));
2304  pushLink(new iDynLink(2.175, 0.00144, 0.06417, -0.00039, 7591.073e-6, -67.260e-6, 2.267e-6,1423.0245e-6,36.37258e-6,7553.849e-6, 0.0, -0.2236, M_PI/2.0, -M_PI/2.0, -79.0*CTRL_DEG2RAD, 79.0*CTRL_DEG2RAD));
2305  pushLink(new iDynLink(1.264, 0.1059, 0.00182, 0.00211, 998.950e-6,-185.699e-6,-63.147e-6, 4450.537e-6, 0.786e-6,4207.657e-6, -0.213, 0.0, M_PI, M_PI/2.0, -125.0*CTRL_DEG2RAD, 23.0*CTRL_DEG2RAD));
2306  pushLink(new iDynLink(0.746, -0.0054, 0.00163, 0.0172, 633.230e-6, -7.081e-6, 41.421e-6, 687.760e-6, 20.817e-6, 313.897e-6, 0.0, 0.0, -M_PI/2.0, 0.0, -42.0*CTRL_DEG2RAD, 21.0*CTRL_DEG2RAD));
2307  pushLink(new iDynLink(0.010, 0, 0, 0, 0, 0, 0, 0, 0, 0, -0.041, 0.0, 0.0, 0.0, -24.0*CTRL_DEG2RAD, 24.0*CTRL_DEG2RAD));
2308  }
2309 #endif
2310 
2311 #ifdef NO_JOINT_COSTRAINTS
2312  this->setAllConstraints(false);
2313 #endif
2314 }
2315 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
2316 bool iCubLegDyn::alignJointsBounds(const deque<IControlLimits*> &lim)
2317 {
2318  if (lim.size()<1)
2319  return false;
2320 
2321  IControlLimits &limLeg=*lim[0];
2322 
2323  unsigned int iLeg;
2324  double min, max;
2325 
2326  for (iLeg=0; iLeg<getN(); iLeg++)
2327  {
2328  if (!limLeg.getLimits(iLeg,&min,&max))
2329  return false;
2330 
2331  (*this)[iLeg].setMin(CTRL_DEG2RAD*min);
2332  (*this)[iLeg].setMax(CTRL_DEG2RAD*max);
2333  }
2334 
2335  return true;
2336 }
2337 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
2338 
2340 // ICUB LEG DYNV2
2342 
2343 iCubLegDynV2::iCubLegDynV2()
2344 {
2345  allocate("right");
2346  setIterMode(KINFWD_WREBWD);
2347 }
2348 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
2349 iCubLegDynV2::iCubLegDynV2(const string &_type,const ChainComputationMode _mode)
2350 {
2351  allocate(_type);
2352  setIterMode(_mode);
2353 }
2354 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
2355 iCubLegDynV2::iCubLegDynV2(const iCubLegDyn &leg)
2356 {
2357  clone(leg);
2358 }
2359 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
2360 
2428 void iCubLegDynV2::allocate(const string &_type)
2429 {
2430  iDynLimb::allocate(_type);
2431 
2432  Matrix H0(4,4);
2433  H0.eye();
2434  setH0(H0);
2435 
2436 #ifdef LEGS_NO_WEIGHT
2437  if(getType()="right")
2438  {
2439  //create iDynLink from parameters calling
2440  //pushLink(new iDynLink(mass,HC,I,A,D,alfa,offset,min,max));
2441  // m rcx rcy rcZ I1 I2 I3 I4 I5 I6 A D alpha offset
2442  pushLink(new iDynLink(0, 80.4310e-6, -6.91643e-3, 2.30470e-3, 0, 0, 0, 0, 0, 0, 0.0, 0.0, M_PI/2.0, M_PI/2.0, -44.0*CTRL_DEG2RAD, 132.0*CTRL_DEG2RAD));
2443  pushLink(new iDynLink(0, -31.3229e-6, -1.09640e-3, 59.8624e-3, 0, 0, 0, 0, 0, 0, 0.0, M_PI/2.0, M_PI/2.0, -17.0*CTRL_DEG2RAD, 119.0*CTRL_DEG2RAD));
2444  pushLink(new iDynLink(0, 3.1143e-3, 60.0431e-3, -1.3437e-3, 0, 0, 0, 0, 0, 0, -0.0009175, 0.234545, -M_PI/2.0, -M_PI/2.0, -79.0*CTRL_DEG2RAD, 79.0*CTRL_DEG2RAD));
2445  pushLink(new iDynLink(0, 124.3956e-3, -45.0091e-6, -6.2408e-3, 0, 0, 0, 0, 0, 0, -0.2005, 0.0, M_PI, M_PI/2.0, -125.0*CTRL_DEG2RAD, 23.0*CTRL_DEG2RAD));
2446  pushLink(new iDynLink(0, -5.3995e-6, -0.858761e-3, -10.2169e-3, 0, 0, 0, 0, 0, 0, 0.0, 0.0, M_PI/2.0, 0.0, -42.0*CTRL_DEG2RAD, 21.0*CTRL_DEG2RAD));
2447  pushLink(new iDynLink(0, 32.41539-3, 613.9310e-6, 24.0690e-3, 0, 0, 0, 0, 0, 0, -0.0685, 0.0035, M_PI, 0.0, -24.0*CTRL_DEG2RAD, 24.0*CTRL_DEG2RAD));
2448 
2449  }
2450  else
2451  {
2452  pushLink(new iDynLink(0, 80.4310e-6, -6.91643e-3, -2.30470e-3, 0, 0, 0, 0, 0, 0, 0.0, 0.0, -M_PI/2.0, M_PI/2.0, -44.0*CTRL_DEG2RAD, 132.0*CTRL_DEG2RAD));
2453  pushLink(new iDynLink(0, 31.3229e-6, -1.09640e-3, -59.8624e-3, 0, 0, 0, 0, 0, 0, 0.0, 0.0, -M_PI/2.0, M_PI/2.0, -17.0*CTRL_DEG2RAD, 119.0*CTRL_DEG2RAD));
2454  pushLink(new iDynLink(0, 3.1143e-3, 60.0431e-3, 1.3437e-3, 0, 0, 0, 0, 0, 0, -0.0009175, -0.234545, M_PI/2.0, -M_PI/2.0, -79.0*CTRL_DEG2RAD, 79.0*CTRL_DEG2RAD));
2455  pushLink(new iDynLink(0, 124.3956e-3, -45.0091e-6, 6.2408e-3, 0, 0, 0, 0, 0, 0, -0.2005, 0.0, M_PI, M_PI/2.0, -125.0*CTRL_DEG2RAD, 23.0*CTRL_DEG2RAD));
2456  pushLink(new iDynLink(0, 5.3995e-6, -0.858761e-3, 10.2169e-3, 0, 0, 0, 0, 0, 0, 0.0, 0.0, -M_PI/2.0, 0.0, -42.0*CTRL_DEG2RAD, 21.0*CTRL_DEG2RAD));
2457  pushLink(new iDynLink(0, 32.41539-3, -613.9310e-6, 24.0690e-3, 0, 0, 0, 0, 0, 0, -0.0685, -0.0035, 0, 0.0, -24.0*CTRL_DEG2RAD, 24.0*CTRL_DEG2RAD));
2458 
2459  }
2460 #else
2461  if(getType()=="right")
2462  {
2463  //create iDynLink from parameters calling
2464  //pushLink(new iDynLink(mass,HC,I,A,D,alfa,offset,min,max));
2465 
2466  pushLink(new iDynLink(0.695, 80.4310e-6, -6.91643e-3, 2.30470e-3, 4470.534e-6, -22.209e-6, -13.9712e-6, 3250.8524e-6, -47.7629e-6, 4904.8747e-6, 0.0, 0.0, M_PI/2.0, M_PI/2.0, -44.0*CTRL_DEG2RAD, 132.0*CTRL_DEG2RAD));
2467  pushLink(new iDynLink(0.982, -31.3229e-6, -1.09640e-3, 59.8624e-3, 27271.4e-6, -1.2116e-6, 11.5713e-6, 25364.6e-6, -491.0806e-6, 4173.9e-6, 0.0, 0.0, M_PI/2.0, M_PI/2.0, -17.0*CTRL_DEG2RAD, 119.0*CTRL_DEG2RAD));
2468  pushLink(new iDynLink(1.522, 3.1143e-3, 60.0431e-3, -1.3437e-3, 45491.7e-6, -940.9711e-6, -53.7405e-6, 10040.0e-6, -1203.0e-6, 45825.5e-6, -0.0009175, 0.234545, -M_PI/2.0, -M_PI/2.0, -79.0*CTRL_DEG2RAD, 79.0*CTRL_DEG2RAD));
2469  pushLink(new iDynLink(2.032, 124.3956e-3, -45.0091e-6, -6.2408e-3, 15455.9e-6, -1390.3e-6, 3945.4e-6, 70277.0e-6, -719.9501e-6, 65964.5e-6, -0.2005, 0.0, M_PI, M_PI/2.0, -125.0*CTRL_DEG2RAD, 23.0*CTRL_DEG2RAD));
2470  pushLink(new iDynLink(0.643, -26.5892e-6, -0.82253e-3, -10.2574e-3, 4765.926e-3, 9.7426e-6, -26.8085e-6, 4093.5e-6, 353.7961e-6, 3668.8e-6, 0.0, 0.0, M_PI/2.0, 0.0, -42.0*CTRL_DEG2RAD, 21.0*CTRL_DEG2RAD));
2471  pushLink(new iDynLink(0.861, 32.41539e-3, 613.9310e-6, 24.0690e-3, 1584.66e-6, 14.357686e-6, 21.079452e-6, 2014.6957e-6, 17.241674e-6, 977.22556e-6, -0.0685, 0.0035, M_PI, 0.0, -24.0*CTRL_DEG2RAD, 24.0*CTRL_DEG2RAD));
2472 
2473  }
2474  else
2475  {
2476 
2477  pushLink(new iDynLink(0.695, 80.4310e-6, -6.91643e-3, -2.30470e-3, 4470.534e-6, -22.209e-6, 13.9712e-6, 3250.8524e-6, 47.7629e-6, 4904.8747e-6, 0.0, 0.0, -M_PI/2.0, M_PI/2.0, -44.0*CTRL_DEG2RAD, 132.0*CTRL_DEG2RAD));
2478  pushLink(new iDynLink(0.982, 31.3229e-6, -1.09640e-3, -59.8624e-3, 27271.4e-6, 1.2116e-6, 11.5713e-6, 25364.6e-6, 491.0806e-6, 4173.9e-6, 0.0, 0.0, -M_PI/2.0, M_PI/2.0, -17.0*CTRL_DEG2RAD, 119.0*CTRL_DEG2RAD));
2479  pushLink(new iDynLink(1.522, 3.1143e-3, 60.0431e-3, 1.3437e-3, 45491.7e-6, -940.9711e-6, 53.7405e-6, 10040.0e-6, 1203.0e-6, 45825.5e-6, -0.0009175, -0.234545, M_PI/2.0, -M_PI/2.0, -79.0*CTRL_DEG2RAD, 79.0*CTRL_DEG2RAD));
2480  pushLink(new iDynLink(2.032, 124.3956e-3, -45.0091e-6, 6.2408e-3, 15455.9e-6, -1390.3e-6, -3945.4e-6, 70277.0e-6, 719.9501e-6, 65964.5e-6, -0.2005, 0.0, M_PI, M_PI/2.0, -125.0*CTRL_DEG2RAD, 23.0*CTRL_DEG2RAD));
2481  pushLink(new iDynLink(0.643, 26.5892e-6, -0.82253e-3, 10.2574e-3, 4765.926e-3, 9.7426e-6, 26.8085e-6, 4093.5e-6, -353.7961e-6, 3668.8e-6, 0.0, 0.0, -M_PI/2.0, 0.0, -42.0*CTRL_DEG2RAD, 21.0*CTRL_DEG2RAD));
2482  pushLink(new iDynLink(0.861, 32.4154e-3, -613.9310e-6, 24.0690e-3, 1584.66e-6, -14.357866e-6, 21.079452e-6, 2014.6957e-6, -17.241674e-6, 977.22556e-6, -0.0685, -0.0035, 0, 0.0, -24.0*CTRL_DEG2RAD, 24.0*CTRL_DEG2RAD));
2483 
2484  }
2485 #endif
2486 
2487 #ifdef NO_JOINT_COSTRAINTS
2488  this->setAllConstraints(false);
2489 #endif
2490 }
2491 
2492 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
2493 bool iCubLegDynV2::alignJointsBounds(const deque<IControlLimits*> &lim)
2494 {
2495  if (lim.size()<1)
2496  return false;
2497 
2498  IControlLimits &limLeg=*lim[0];
2499 
2500  unsigned int iLeg;
2501  double min, max;
2502 
2503  for (iLeg=0; iLeg<getN(); iLeg++)
2504  {
2505  if (!limLeg.getLimits(iLeg,&min,&max))
2506  return false;
2507 
2508  (*this)[iLeg].setMin(CTRL_DEG2RAD*min);
2509  (*this)[iLeg].setMax(CTRL_DEG2RAD*max);
2510  }
2511 
2512  return true;
2513 }
2514 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
2515 
2517 // ICUB INERTIAL SENSOR DYN
2519 
2520 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
2521 iCubNeckInertialDyn::iCubNeckInertialDyn(const ChainComputationMode _mode)
2522 {
2523  allocate("head");
2524  setIterMode(_mode);
2525 }
2526 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
2527 iCubNeckInertialDyn::iCubNeckInertialDyn(const iCubNeckInertialDyn &sensor)
2528 {
2529  clone(sensor);
2530 }
2531 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
2532 void iCubNeckInertialDyn::allocate(const string &_type)
2533 {
2534  iDynLimb::allocate(_type);
2535 
2536  Matrix H0(4,4);
2537  H0.eye();
2538  setH0(H0);
2539 
2540  pushLink(new iDynLink(0.27017604, -30.535917e-3, 0, -0.23571261e-3, 100.46346e-6, -0.17765781e-6, 0.44914333e-6, 45.425961e-6, -0.12682862e-6, 0, 0.033, 0.0, M_PI/2.0, M_PI/2.0, -40.0*CTRL_DEG2RAD, 30.0*CTRL_DEG2RAD));
2541  pushLink(new iDynLink(0.27230552, 0.0, 4.3752947e-3, 5.4544215e-3, 142.82339e-6, -0.0059261471e-6, -0.0022006663e-6, 82.884917e-6, -9.1321119e-6, 87.620338e-6, 0.0, 0.001, -M_PI/2.0, -M_PI/2.0, -70.0*CTRL_DEG2RAD, 60.0*CTRL_DEG2RAD));
2542  pushLink(new iDynLink(1.3368659, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0.0225, 0.1005, -M_PI/2.0, M_PI/2.0, -55.0*CTRL_DEG2RAD, 55.0*CTRL_DEG2RAD));
2543  // NOTE: VERIFY THE DYNAMIC PARAMETERS OF THE HEAD (WRITTEN ABOVE)
2544 
2545  // virtual links that describe T_nls (https://icub-tech-iit.github.io/documentation/icub_kinematics/icub-forward-kinematics/icub-forward-kinematics-imu/)
2546  // The link below must have no dynamic parameters.
2547  pushLink(new iDynLink(0 , 0, 0, 0, 0, 0, 0, 0, 0, 0, 0.0, 0.0066, M_PI/2.0, 0.0, 0.0, 0.0));
2548 
2549  blockLink(3,0.0);
2550 
2551 #ifdef NO_JOINT_COSTRAINTS
2552  this->setAllConstraints(false);
2553 #endif
2554 }
2555 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
2556 bool iCubNeckInertialDyn::alignJointsBounds(const deque<IControlLimits*> &lim)
2557 {
2558  if (lim.size()<1)
2559  return false;
2560 
2561  IControlLimits &limHead =*lim[0];
2562 
2563  unsigned int iHead;
2564  double min, max;
2565 
2566  // only the neck: the sensor is in a virtual link
2567  for (iHead=0; iHead<3; iHead++)
2568  {
2569  if (!limHead.getLimits(iHead,&min,&max))
2570  return false;
2571 
2572  (*this)[iHead].setMin(CTRL_DEG2RAD*min);
2573  (*this)[iHead].setMax(CTRL_DEG2RAD*max);
2574  }
2575 
2576  return true;
2577 }
2578 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
2579 
2580 
2582 // ICUB INERTIAL SENSOR DYN V2
2584 
2585 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
2586 iCubNeckInertialDynV2::iCubNeckInertialDynV2(const ChainComputationMode _mode, const string &_type)
2587 {
2588  allocate(_type);
2589  setIterMode(_mode);
2590 }
2591 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
2592 iCubNeckInertialDynV2::iCubNeckInertialDynV2(const iCubNeckInertialDynV2 &sensor)
2593 {
2594  clone(sensor);
2595 }
2596 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
2597 void iCubNeckInertialDynV2::allocate(const string &_type)
2598 {
2599  iDynLimb::allocate("head");
2600 
2601  Matrix H0(4,4);
2602  H0.eye();
2603  setH0(H0);
2604 
2605  //sorry: mass and dynamic parameters have still to be calculated for the V2 head
2606  pushLink(new iDynLink(0*mhd, 0,0,0, 0,0,0,0,0,0, 0.0095, 0.0, M_PI/2.0, M_PI/2.0, -40.0*CTRL_DEG2RAD, 30.0*CTRL_DEG2RAD));
2607  pushLink(new iDynLink(0*mhd, 0,0,0, 0,0,0,0,0,0, 0.0, 0.0, -M_PI/2.0, -M_PI/2.0, -70.0*CTRL_DEG2RAD, 60.0*CTRL_DEG2RAD));
2608  pushLink(new iDynLink(1.3368659*mhd, 0,0,0, 0,0,0,0,0,0, 0.0185, 0.1108, -M_PI/2.0, M_PI/2.0, -55.0*CTRL_DEG2RAD, 55.0*CTRL_DEG2RAD));
2609  // NOTE: VERIFY THE DYNAMIC PARAMETERS OF THE HEAD (WRITTEN ABOVE)
2610 
2611  // virtual links that describe T_nls (https://icub-tech-iit.github.io/documentation/icub_kinematics/icub-forward-kinematics/icub-forward-kinematics-imu/)
2612  // The link below must have no dynamic parameters.
2613  pushLink(new iDynLink(0*mhd, 0,0,0, 0,0,0,0,0,0, 0.0, 0.0066, M_PI/2.0, 0.0, 0.0, 0.0));
2614 
2615  blockLink(3,0.0);
2616 
2617  if (_type=="v2.6") // imurfe
2618  {
2619  Matrix HN=zeros(4,4);
2620  HN(0,3)=0.0323779;
2621  HN(1,3)=-0.0139537;
2622  HN(2,3)=0.072;
2623  HN(1,0)=1.0;
2624  HN(0,1)=1.0;
2625  HN(2,2)=-1.0;
2626  HN(3,3)=1.0;
2627  setHN(getHN()*HN);
2628  }
2629 
2630 #ifdef NO_JOINT_COSTRAINTS
2631  this->setAllConstraints(false);
2632 #endif
2633 }
2634 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
2635 bool iCubNeckInertialDynV2::alignJointsBounds(const deque<IControlLimits*> &lim)
2636 {
2637  if (lim.size()<1)
2638  return false;
2639 
2640  IControlLimits &limHead =*lim[0];
2641 
2642  unsigned int iHead;
2643  double min, max;
2644 
2645  // only the neck: the sensor is in a virtual link
2646  for (iHead=0; iHead<3; iHead++)
2647  {
2648  if (!limHead.getLimits(iHead,&min,&max))
2649  return false;
2650 
2651  (*this)[iHead].setMin(CTRL_DEG2RAD*min);
2652  (*this)[iHead].setMax(CTRL_DEG2RAD*max);
2653  }
2654 
2655  return true;
2656 }
2657 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
2658 
CTRL_DEG2RAD
#define CTRL_DEG2RAD
Definition: XSensMTx.cpp:26
M_PI
#define M_PI
Definition: XSensMTx.cpp:24
iCub::iDyn::OneChainNewtonEuler::BackwardWrenchFromEnd
void BackwardWrenchFromEnd()
[classic] Base function for backward of classical Newton-Euler.
Definition: iDynInv.cpp:1883
iCub::iDyn::OneChainNewtonEuler::getVelAccAfterForward
bool getVelAccAfterForward(unsigned int i, yarp::sig::Vector &w, yarp::sig::Vector &dw, yarp::sig::Vector &dwM, yarp::sig::Vector &ddp, yarp::sig::Vector &ddpC) const
Useful to debug, getting the intermediate computations after the forward phase.
Definition: iDynInv.cpp:1732
iCub::iDyn::OneChainNewtonEuler::getVelAccEnd
void getVelAccEnd(yarp::sig::Vector &w, yarp::sig::Vector &dw, yarp::sig::Vector &ddp) const
This method is used by iDynChain to retrieve kinematic information for connection with one or more iD...
Definition: iDynInv.cpp:1774
iCub::iDyn::OneChainNewtonEuler::ForwardKinematicFromBase
void ForwardKinematicFromBase()
[classic/inverse] Base function for forward of classical Newton-Euler.
Definition: iDynInv.cpp:1851
iCub::iDyn::OneChainNewtonEuler::initKinematicBase
bool initKinematicBase(const yarp::sig::Vector &w0, const yarp::sig::Vector &dw0, const yarp::sig::Vector &ddp0)
[classic] Initialize the base with measured or known kinematics variables
Definition: iDynInv.cpp:1818
iCub::iDyn::OneChainNewtonEuler::initWrenchEnd
bool initWrenchEnd(const yarp::sig::Vector &F0, const yarp::sig::Vector &Mu0)
[classic] Initialize the end-effector finalLink with measured or known wrench
Definition: iDynInv.cpp:1828
iCub::iDyn::OneChainNewtonEuler::setMode
void setMode(const NewEulMode _mode)
Definition: iDynInv.cpp:1806
iCub::iDyn::OneChainNewtonEuler::getMode
NewEulMode getMode() const
Definition: iDynInv.cpp:1844
iCub::iDyn::OneChainNewtonEuler::getWrenchEnd
void getWrenchEnd(yarp::sig::Vector &F, yarp::sig::Vector &Mu) const
This method is used by iDynChain to retrieve wrench information for connection with one or more iDynL...
Definition: iDynInv.cpp:1787
iCub::iDyn::OneChainNewtonEuler::neChain
OneLinkNewtonEuler ** neChain
the chain of links/frames for Newton-Euler computations
Definition: iDynInv.h:890
iCub::iDyn::OneChainNewtonEuler::BackwardKinematicFromEnd
void BackwardKinematicFromEnd()
[inverse] Base function for forward of classical Newton-Euler.
Definition: iDynInv.cpp:1867
iCub::iDyn::OneChainNewtonEuler::initWrenchBase
bool initWrenchBase(const yarp::sig::Vector &F0, const yarp::sig::Vector &Mu0)
[inverse] Initialize the base with measured or known wrench
Definition: iDynInv.cpp:1833
iCub::iDyn::OneChainNewtonEuler::getWrenchBase
void getWrenchBase(yarp::sig::Vector &F, yarp::sig::Vector &Mu) const
This method is used by iDynChain to retrieve wrench information for connection with one or more iDynL...
Definition: iDynInv.cpp:1781
iCub::iDyn::OneChainNewtonEuler::getVelAccBase
void getVelAccBase(yarp::sig::Vector &w, yarp::sig::Vector &dw, yarp::sig::Vector &ddp) const
This method is used by iDynChain to retrieve kinematic information for connection with one or more iD...
Definition: iDynInv.cpp:1767
iCub::iDyn::OneChainNewtonEuler::ForwardWrenchFromBase
void ForwardWrenchFromBase()
[inverse] Base function for inverse Newton-Euler: from the i-th link to the end, forward of forces an...
Definition: iDynInv.cpp:1908
iCub::iDyn::OneChainNewtonEuler::getWrenchAfterForward
bool getWrenchAfterForward(unsigned int i, yarp::sig::Vector &F, yarp::sig::Vector &Mu) const
Useful to debug, getting the intermediate computations after the forward phase.
Definition: iDynInv.cpp:1751
iCub::iDyn::OneChainNewtonEuler::initKinematicEnd
bool initKinematicEnd(const yarp::sig::Vector &w0, const yarp::sig::Vector &dw0, const yarp::sig::Vector &ddp0)
[inverse] Initialize the end-effector finalLink with measured or known kinematics variables
Definition: iDynInv.cpp:1823
iCub::iDyn::OneLinkNewtonEuler::getForce
virtual const yarp::sig::Vector & getForce() const
Definition: iDynInv.cpp:348
iCub::iDyn::OneLinkNewtonEuler::BackwardWrench
void BackwardWrench(OneLinkNewtonEuler *next)
[Backward Newton-Euler] Compute F, Mu, Tau
Definition: iDynInv.cpp:737
iCub::iDyn::OneLinkNewtonEuler::getMoment
virtual const yarp::sig::Vector & getMoment(bool isBase=false) const
Definition: iDynInv.cpp:350
iCub::iDyn::OneLinkNewtonEuler::getTorque
virtual double getTorque() const
Definition: iDynInv.cpp:352
iCub::iDyn::OneLinkNewtonEuler::ForwardKinematics
void ForwardKinematics(OneLinkNewtonEuler *prev)
[Forward Newton-Euler] Compute w, dw, ddp, ddpC, dwM
Definition: iDynInv.cpp:720
iCub::iDyn::iCubArmDyn
A class for defining the 7-DOF iCub Arm in the iDyn framework.
Definition: iDyn.h:1328
iCub::iDyn::iCubArmDyn::iCubArmDyn
iCubArmDyn()
Default constructor.
Definition: iDyn.cpp:1950
iCub::iDyn::iCubArmDyn::alignJointsBounds
virtual bool alignJointsBounds(const std::deque< yarp::dev::IControlLimits * > &lim)
Alignes the Arm joints bounds with current values set aboard the iCub.
Definition: iDyn.cpp:2016
iCub::iDyn::iCubArmDyn::allocate
virtual void allocate(const std::string &_type)
Definition: iDyn.cpp:1967
iCub::iDyn::iCubArmNoTorsoDyn
A class for defining the 7-DOF iCub Arm in the iDyn framework.
Definition: iDyn.h:1369
iCub::iDyn::iCubArmNoTorsoDyn::iCubArmNoTorsoDyn
iCubArmNoTorsoDyn()
Default constructor.
Definition: iDyn.cpp:2060
iCub::iDyn::iCubArmNoTorsoDyn::alignJointsBounds
virtual bool alignJointsBounds(const std::deque< yarp::dev::IControlLimits * > &lim)
Alignes the Arm joints bounds with current values set aboard the iCub.
Definition: iDyn.cpp:2117
iCub::iDyn::iCubArmNoTorsoDyn::allocate
virtual void allocate(const std::string &_type)
Definition: iDyn.cpp:2077
iCub::iDyn::iCubLegDynV2::allocate
virtual void allocate(const std::string &_type)
commented by Traversaro, 2 sept 2013 void iCubLegDynV2::allocate(const string &_type) { iDynLimb::all...
Definition: iDyn.cpp:2428
iCub::iDyn::iCubLegDynV2::iCubLegDynV2
iCubLegDynV2()
Default constructor.
Definition: iDyn.cpp:2343
iCub::iDyn::iCubLegDynV2::alignJointsBounds
virtual bool alignJointsBounds(const std::deque< yarp::dev::IControlLimits * > &lim)
Alignes the Leg joints bounds with current values set aboard the iCub.
Definition: iDyn.cpp:2493
iCub::iDyn::iCubLegDyn
A class for defining the 6-DOF iCub Leg.
Definition: iDyn.h:1452
iCub::iDyn::iCubLegDyn::iCubLegDyn
iCubLegDyn()
Default constructor.
Definition: iDyn.cpp:2238
iCub::iDyn::iCubLegDyn::allocate
virtual void allocate(const std::string &_type)
Definition: iDyn.cpp:2255
iCub::iDyn::iCubLegDyn::alignJointsBounds
virtual bool alignJointsBounds(const std::deque< yarp::dev::IControlLimits * > &lim)
Alignes the Leg joints bounds with current values set aboard the iCub.
Definition: iDyn.cpp:2316
iCub::iDyn::iCubNeckInertialDynV2
A class for defining the 3-DOF Inertia Sensor Kinematics (V2 HEAD)
Definition: iDyn.h:1566
iCub::iDyn::iCubNeckInertialDynV2::iCubNeckInertialDynV2
iCubNeckInertialDynV2(const ChainComputationMode _mode=KINBWD_WREBWD, const std::string &_type="v2.5")
Default constructor.
Definition: iDyn.cpp:2586
iCub::iDyn::iCubNeckInertialDynV2::allocate
virtual void allocate(const std::string &_type)
Definition: iDyn.cpp:2597
iCub::iDyn::iCubNeckInertialDynV2::alignJointsBounds
virtual bool alignJointsBounds(const std::deque< yarp::dev::IControlLimits * > &lim)
Alignes the Inertial Sensor joints bounds with current values set aboard the iCub.
Definition: iDyn.cpp:2635
iCub::iDyn::iCubNeckInertialDyn
A class for defining the 3-DOF Inertia Sensor Kinematics.
Definition: iDyn.h:1533
iCub::iDyn::iCubNeckInertialDyn::iCubNeckInertialDyn
iCubNeckInertialDyn(const ChainComputationMode _mode=KINBWD_WREBWD)
Default constructor.
Definition: iDyn.cpp:2521
iCub::iDyn::iCubNeckInertialDyn::allocate
virtual void allocate(const std::string &_type)
Definition: iDyn.cpp:2532
iCub::iDyn::iCubNeckInertialDyn::alignJointsBounds
virtual bool alignJointsBounds(const std::deque< yarp::dev::IControlLimits * > &lim)
Alignes the Inertial Sensor joints bounds with current values set aboard the iCub.
Definition: iDyn.cpp:2556
iCub::iDyn::iCubTorsoDyn
A class for defining the 3-DOF iCub Torso in the iDyn framework.
Definition: iDyn.h:1410
iCub::iDyn::iCubTorsoDyn::allocate
virtual void allocate(const std::string &_type)
Definition: iDyn.cpp:2166
iCub::iDyn::iCubTorsoDyn::alignJointsBounds
virtual bool alignJointsBounds(const std::deque< yarp::dev::IControlLimits * > &lim)
Alignes the Torso joints bounds with current values set aboard the iCub.
Definition: iDyn.cpp:2202
iCub::iDyn::iCubTorsoDyn::iCubTorsoDyn
iCubTorsoDyn()
Default constructor.
Definition: iDyn.cpp:2149
iCub::iDyn::iDynChain
A Base class for defining a Serial Link Chain, using dynamics and kinematics.
Definition: iDyn.h:533
iCub::iDyn::iDynChain::computeGeoJacobian
yarp::sig::Matrix computeGeoJacobian(const unsigned int iLinkN, const yarp::sig::Matrix &Pn)
Compute the Jacobian from link 0 to iLinkN.
iCub::iDyn::iDynChain::getDenHart
yarp::sig::Matrix getDenHart(unsigned int i)
Return the Denavit-Hartenberg matrix of the i-th link in the chain.
Definition: iDyn.cpp:1467
iCub::iDyn::iDynChain::getAngAcc
yarp::sig::Vector getAngAcc(const unsigned int i) const
Returns the i-th link angular acceleration.
Definition: iDyn.cpp:744
iCub::iDyn::iDynChain::getLinAccCOM
const yarp::sig::Vector & getLinAccCOM(const unsigned int i) const
Returns the i-th link linear acceleration of the COM.
Definition: iDyn.cpp:722
iCub::iDyn::iDynChain::getForceMomentEndEff
yarp::sig::Vector getForceMomentEndEff() const
Returns the end effector force-moment as a single (6x1) vector.
Definition: iDyn.cpp:1272
iCub::iDyn::iDynChain::setMasses
bool setMasses(yarp::sig::Vector _m)
Set the link masses at once.
Definition: iDyn.cpp:807
iCub::iDyn::iDynChain::zero0
const yarp::sig::Vector zero0
Definition: iDyn.h:556
iCub::iDyn::iDynChain::getLinVelCOM
yarp::sig::Vector getLinVelCOM(const unsigned int i) const
Returns the i-th link linear velocity of the COM.
Definition: iDyn.cpp:700
iCub::iDyn::iDynChain::TESTING_computeCOMJacobian
yarp::sig::Matrix TESTING_computeCOMJacobian(const unsigned int iLink)
Compute the Jacobian of the COM of link indexed iLink.
Definition: iDyn.cpp:1478
iCub::iDyn::iDynChain::setStaticParameters
bool setStaticParameters(const unsigned int i, const double _m, const yarp::sig::Matrix &_HC)
Set the dynamic parameters of the i-th Link if the chain is in a static situation (inertia is null).
Definition: iDyn.cpp:905
iCub::iDyn::iDynChain::setIterMode
void setIterMode(const ChainComputationMode mode=KINFWD_WREBWD)
Set the computation mode during recursive computation of kinematics (w,dw,d2p,d2pC) and wrench variab...
Definition: iDyn.cpp:1302
iCub::iDyn::iDynChain::computeKinematicNewtonEuler
void computeKinematicNewtonEuler()
Calls the proper method to compute kinematics variables: either ForwardKinematicFromBase() or Backwar...
Definition: iDyn.cpp:992
iCub::iDyn::iDynChain::setMass
bool setMass(const unsigned int i, const double _m)
Set the i-th link mass.
Definition: iDyn.cpp:833
iCub::iDyn::iDynChain::initKinematicNewtonEuler
bool initKinematicNewtonEuler(const yarp::sig::Vector &w0, const yarp::sig::Vector &dw0, const yarp::sig::Vector &ddp0)
Calls the proper method to set kinematics variables in OneChainNewtonEuler: either initKinematicBase(...
Definition: iDyn.cpp:1144
iCub::iDyn::iDynChain::getForce
const yarp::sig::Vector & getForce(const unsigned int iLink) const
Returns the i-th link force.
Definition: iDyn.cpp:766
iCub::iDyn::iDynChain::getMasses
yarp::sig::Vector getMasses() const
Returns the link masses as a vector.
Definition: iDyn.cpp:799
iCub::iDyn::iDynChain::setIterModeKinematic
void setIterModeKinematic(const ChainIterationMode _iterateMode_kinematics=FORWARD)
Set the iteration direction during recursive computation of kinematics variables (w,...
Definition: iDyn.cpp:1282
iCub::iDyn::iDynChain::getForces
yarp::sig::Matrix getForces() const
Returns the links forces as a matrix, where the i-th col is the i-th force.
Definition: iDyn.cpp:859
iCub::iDyn::iDynChain::getFrameKinematic
void getFrameKinematic(unsigned int i, yarp::sig::Vector &w, yarp::sig::Vector &dw, yarp::sig::Vector &ddp)
Get the kinematic information of the i-th frame in the OneChainNewtonEuler associated to the current ...
Definition: iDyn.cpp:1036
iCub::iDyn::iDynChain::computeWrenchNewtonEuler
void computeWrenchNewtonEuler()
Calls the proper method to compute wrench variables: either BackwardWrenchFromEnd() or ForwardWrenchF...
Definition: iDyn.cpp:1000
iCub::iDyn::iDynChain::computeCcTorques
yarp::sig::Vector computeCcTorques()
Compute the torques due to centrifugal and coriolis effects considering only the active joints.
Definition: iDyn.cpp:1673
iCub::iDyn::iDynChain::curr_ddq
yarp::sig::Vector curr_ddq
q acc
Definition: iDyn.h:551
iCub::iDyn::iDynChain::initNewtonEuler
bool initNewtonEuler()
Initialize the Newton-Euler method by setting the base (virtual link) velocity and accelerations ( w0...
Definition: iDyn.cpp:1099
iCub::iDyn::iDynChain::NE
OneChainNewtonEuler * NE
pointer to OneChainNewtonEuler class, to be used for computing forces and torques
Definition: iDyn.h:554
iCub::iDyn::iDynChain::setD2Ang
yarp::sig::Vector setD2Ang(const yarp::sig::Vector &ddq)
Sets the free joint angles acceleration to values of ddq[i].
iCub::iDyn::iDynChain::initWrenchNewtonEuler
bool initWrenchNewtonEuler(const yarp::sig::Vector &Fend, const yarp::sig::Vector &Muend)
Calls the proper method to set wrench variables in OneChainNewtonEuler: either initKinematicBase() or...
Definition: iDyn.cpp:1174
iCub::iDyn::iDynChain::getJointBoundMin
yarp::sig::Vector getJointBoundMin()
Returns a list containing the min value for each joint.
Definition: iDyn.cpp:610
iCub::iDyn::iDynChain::prepareNewtonEuler
void prepareNewtonEuler(const NewEulMode NewEulMode_s=DYNAMIC)
Prepare for the Newton-Euler recursive computation of forces and torques.
Definition: iDyn.cpp:916
iCub::iDyn::iDynChain::getInertia
yarp::sig::Matrix getInertia(const unsigned int i) const
Returns the i-th link inertia matrix.
Definition: iDyn.cpp:847
iCub::iDyn::iDynChain::setIterModeWrench
void setIterModeWrench(const ChainIterationMode _iterateMode_wrench=BACKWARD)
Set the iteration direction during recursive computation of wrench variables (F,Mu,...
Definition: iDyn.cpp:1287
iCub::iDyn::iDynChain::getD2Ang
yarp::sig::Vector getD2Ang()
Returns the current free joint angles acceleration.
Definition: iDyn.cpp:597
iCub::iDyn::iDynChain::build
virtual void build()
Build chains and lists.
Definition: iDyn.cpp:493
iCub::iDyn::iDynChain::clone
virtual void clone(const iDynChain &c)
Clone function.
Definition: iDyn.cpp:483
iCub::iDyn::iDynChain::OneChainNewtonEuler
friend class OneChainNewtonEuler
Definition: iDyn.h:534
iCub::iDyn::iDynChain::getMoments
yarp::sig::Matrix getMoments() const
Returns the links moments as a matrix, where the i-th col is the i-th moment.
Definition: iDyn.cpp:867
iCub::iDyn::iDynChain::getMoment
const yarp::sig::Vector & getMoment(const unsigned int iLink) const
Returns the i-th link moment.
Definition: iDyn.cpp:777
iCub::iDyn::iDynChain::getMass
double getMass(const unsigned int i) const
Returns the i-th link mass.
Definition: iDyn.cpp:822
iCub::iDyn::iDynChain::getDAng
yarp::sig::Vector getDAng()
Returns the current free joint angles velocity.
Definition: iDyn.cpp:584
iCub::iDyn::iDynChain::getTorques
yarp::sig::Vector getTorques() const
Returns the links torque as a vector.
Definition: iDyn.cpp:875
iCub::iDyn::iDynChain::getTorquesNewtonEuler
yarp::sig::Vector getTorquesNewtonEuler() const
Returns the links torque as a vector.
Definition: iDyn.cpp:1257
iCub::iDyn::iDynChain::getIterModeWrench
ChainIterationMode getIterModeWrench() const
Get the iteration direction during recursive computation of wrench variables (F,Mu,...
Definition: iDyn.cpp:1297
iCub::iDyn::iDynChain::computeCcGravityTorques
yarp::sig::Vector computeCcGravityTorques(const yarp::sig::Vector &ddp0)
Compute the torques generated by gravity and centrifugal and coriolis forces, considering only the ac...
iCub::iDyn::iDynChain::setDynamicParameters
bool setDynamicParameters(const unsigned int i, const double _m, const yarp::sig::Matrix &_HC, const yarp::sig::Matrix &_I, const double _kr, const double _Fv, const double _Fs, const double _Im)
Set the dynamic parameters of the i-th Link with motor.
iCub::iDyn::iDynChain::setModeNewtonEuler
void setModeNewtonEuler(const NewEulMode NewEulMode_s=DYNAMIC)
Set the computation mode for Newton-Euler (static/dynamic/etc)
Definition: iDyn.cpp:1204
iCub::iDyn::iDynChain::curr_dq
yarp::sig::Vector curr_dq
q vel
Definition: iDyn.h:549
iCub::iDyn::iDynChain::getCOM
yarp::sig::Matrix getCOM(unsigned int iLink)
Return the COM matrix of the i-th link.
Definition: iDyn.cpp:1585
iCub::iDyn::iDynChain::computeGravityTorques
yarp::sig::Vector computeGravityTorques(const yarp::sig::Vector &ddp0)
Compute the torques generated by gravity considering only the active joints.
iCub::iDyn::iDynChain::iterateMode_kinematics
ChainIterationMode iterateMode_kinematics
specifies the 'direction' of recursive computation of kinematics variables (w,dw,d2p): FORWARD,...
Definition: iDyn.h:543
iCub::iDyn::iDynChain::getIterModeKinematic
ChainIterationMode getIterModeKinematic() const
Get the iteration direction during recursive computation of kinematics variables (w,...
Definition: iDyn.cpp:1292
iCub::iDyn::iDynChain::iDynChain
iDynChain()
Default constructor.
Definition: iDyn.cpp:476
iCub::iDyn::iDynChain::setAng
yarp::sig::Vector setAng(const yarp::sig::Vector &q)
Sets the free joint angles to values of q[i].
iCub::iDyn::iDynChain::operator=
iDynChain & operator=(const iDynChain &c)
Copies a Chain object into the current one.
Definition: iDyn.cpp:523
iCub::iDyn::iDynChain::computeMassMatrix
yarp::sig::Matrix computeMassMatrix()
Compute the joint space mass matrix considering only the active joints.
Definition: iDyn.cpp:1605
iCub::iDyn::iDynChain::getJointBoundMax
yarp::sig::Vector getJointBoundMax()
Returns a list containing the max value for each joint.
Definition: iDyn.cpp:621
iCub::iDyn::iDynChain::getTorque
double getTorque(const unsigned int iLink) const
Returns the i-th link torque.
Definition: iDyn.cpp:788
iCub::iDyn::iDynChain::getLinVel
yarp::sig::Vector getLinVel(const unsigned int i) const
Returns the i-th link linear velocity.
Definition: iDyn.cpp:689
iCub::iDyn::iDynChain::refLink
iDynLink * refLink(const unsigned int i)
Returns a pointer to the ith iDynLink.
Definition: iDyn.cpp:755
iCub::iDyn::iDynChain::getWrenchNewtonEuler
void getWrenchNewtonEuler(yarp::sig::Vector &F, yarp::sig::Vector &Mu)
Calls the proper method to get wrench variables in OneChainNewtonEuler either in the base or in the f...
Definition: iDyn.cpp:1072
iCub::iDyn::iDynChain::iterateMode_wrench
ChainIterationMode iterateMode_wrench
specifies the 'direction' of recursive computation of wrenches (F,Mu): FORWARD, BACKWARD
Definition: iDyn.h:545
iCub::iDyn::iDynChain::getFrameWrench
void getFrameWrench(unsigned int i, yarp::sig::Vector &F, yarp::sig::Vector &Mu)
Get the wrench information of the i-th frame in the OneChainNewtonEuler associated to the current iDy...
Definition: iDyn.cpp:1055
iCub::iDyn::iDynChain::setDAng
yarp::sig::Vector setDAng(const yarp::sig::Vector &dq)
Sets the free joint angles velocity to values of dq[i].
iCub::iDyn::iDynChain::getLinAcc
yarp::sig::Vector getLinAcc(const unsigned int i) const
Returns the i-th link linear acceleration.
Definition: iDyn.cpp:711
iCub::iDyn::iDynChain::getAngVel
yarp::sig::Vector getAngVel(const unsigned int i) const
Returns the i-th link angular velocity.
Definition: iDyn.cpp:733
iCub::iDyn::iDynChain::dispose
virtual void dispose()
Dispose.
Definition: iDyn.cpp:508
iCub::iDyn::iDynChain::getMomentsNewtonEuler
yarp::sig::Matrix getMomentsNewtonEuler() const
Returns the links moments as a matrix, where the (i+1)-th col is the i-th moment.
Definition: iDyn.cpp:1242
iCub::iDyn::iDynChain::computeNewtonEuler
bool computeNewtonEuler()
Compute forces and torques with the Newton-Euler recursive algorithm: forward and backward phase are ...
Definition: iDyn.cpp:965
iCub::iDyn::iDynChain::~iDynChain
virtual ~iDynChain()
Destructor.
Definition: iDyn.cpp:518
iCub::iDyn::iDynChain::getKinematicNewtonEuler
void getKinematicNewtonEuler(yarp::sig::Vector &w, yarp::sig::Vector &dw, yarp::sig::Vector &ddp)
Calls the proper method to get kinematics variables in OneChainNewtonEuler either in the base or in t...
Definition: iDyn.cpp:1008
iCub::iDyn::iDynChain::getForcesNewtonEuler
yarp::sig::Matrix getForcesNewtonEuler() const
Returns the links forces as a matrix, where the (i+1)-th col is the i-th force.
Definition: iDyn.cpp:1226
iCub::iDyn::iDynChain::getHCOM
yarp::sig::Matrix getHCOM(unsigned int iLink)
Return the H matrix until the COM of the i-th link.
Definition: iDyn.cpp:1595
iCub::iDyn::iDynLimb
A class for defining a generic Limb within the iDyn framework.
Definition: iDyn.h:1177
iCub::iDyn::iDynLimb::type
std::string type
Definition: iDyn.h:1180
iCub::iDyn::iDynLimb::clone
virtual void clone(const iDynLimb &limb)
Definition: iDyn.cpp:1901
iCub::iDyn::iDynLimb::allocate
virtual void allocate(const std::string &_type)
Definition: iDyn.cpp:1891
iCub::iDyn::iDynLimb::pushLink
void pushLink(iKin::iKinLink &l)
Definition: iDyn.h:1197
iCub::iDyn::iDynLimb::operator=
iDynChain & operator=(const iDynChain &c)
Definition: iDyn.h:1190
iCub::iDyn::iDynLimb::getType
std::string getType()
Returns the Limb type as a string.
Definition: iDyn.h:1300
iCub::iDyn::iDynLimb::~iDynLimb
virtual ~iDynLimb()
Destructor.
Definition: iDyn.cpp:1886
iCub::iDyn::iDynLimb::linkList
std::deque< iDynLink * > linkList
Definition: iDyn.h:1179
iCub::iDyn::iDynLimb::iDynLimb
iDynLimb()
Default constructor: right arm is default.
Definition: iDyn.cpp:1743
iCub::iDyn::iDynLimb::fromLinksProperties
bool fromLinksProperties(const yarp::os::Property &option)
Initializes the Limb from a list of properties wherein links parameters are specified.
Definition: iDyn.cpp:1769
iCub::iDyn::iDynLimb::dispose
virtual void dispose()
Dispose.
Definition: iDyn.cpp:1926
iCub::iKin::iKinChain
A Base class for defining a Serial Link Chain.
Definition: iKinFwd.h:354
iCub::iKin::iKinChain::curr_q
yarp::sig::Vector curr_q
Definition: iKinFwd.h:361
iCub::iKin::iKinChain::getHN
yarp::sig::Matrix getHN() const
Returns HN, the rigid roto-translation matrix from the Nth frame to the end-effector.
Definition: iKinFwd.h:578
iCub::iKin::iKinChain::setHN
bool setHN(const yarp::sig::Matrix &_HN)
Sets HN, the rigid roto-translation matrix from the Nth frame to the end-effector.
Definition: iKinFwd.cpp:580
iCub::iKin::iKinChain::H0
yarp::sig::Matrix H0
Definition: iKinFwd.h:359
iCub::iKin::iKinChain::verbose
unsigned int verbose
Definition: iKinFwd.h:358
iCub::iKin::iKinChain::hash_dof
std::deque< unsigned int > hash_dof
Definition: iKinFwd.h:367
iCub::iKin::iKinChain::hash
std::deque< unsigned int > hash
Definition: iKinFwd.h:366
iCub::iKin::iKinChain::getH
yarp::sig::Matrix getH()
Returns the rigid roto-translation matrix from the root reference frame to the end-effector frame in ...
Definition: iKinFwd.cpp:778
iCub::iKin::iKinChain::quickList
std::deque< iKinLink * > quickList
Definition: iKinFwd.h:364
iCub::iKin::iKinChain::N
unsigned int N
Definition: iKinFwd.h:356
iCub::iKin::iKinChain::getN
unsigned int getN() const
Returns the number of Links belonging to the Chain.
Definition: iKinFwd.h:549
iCub::iKin::iKinChain::blockLink
bool blockLink(const unsigned int i, double Ang)
Blocks the ith Link at the a certain value of its joint angle.
Definition: iKinFwd.cpp:394
iCub::iKin::iKinChain::setH0
bool setH0(const yarp::sig::Matrix &_H0)
Sets H0, the rigid roto-translation matrix from the root reference frame to the 0th frame.
Definition: iKinFwd.cpp:562
iCub::iKin::iKinChain::DOF
unsigned int DOF
Definition: iKinFwd.h:357
iCub::iKin::iKinChain::setAllConstraints
void setAllConstraints(bool _constrained)
Sets the constraint status of all chain links.
Definition: iKinFwd.cpp:498
iCub::iKin::iKinChain::HN
yarp::sig::Matrix HN
Definition: iKinFwd.h:360
iCub::iKin::iKinChain::allList
std::deque< iKinLink * > allList
Definition: iKinFwd.h:363
F
F
Definition: compute_ekf_fast.m:22
zeros
zeros(2, 2) eye(2
f
f
Definition: compute_ekf_sym.m:22
iCub::ctrl::cross
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).
mhd
#define mhd
Definition: iDyn.cpp:40
mra
#define mra
Definition: iDyn.cpp:38
mla
#define mla
Definition: iDyn.cpp:39
iCub::iDyn::notImplemented
void notImplemented(const unsigned int verbose)
Definition: iDyn.cpp:54
iCub::iDyn::ChainComputationMode
ChainComputationMode
Definition: iDynInv.h:69
iCub::iDyn::KINBWD_WREFWD
@ KINBWD_WREFWD
Definition: iDynInv.h:69
iCub::iDyn::KINFWD_WREBWD
@ KINFWD_WREBWD
Definition: iDynInv.h:69
iCub::iDyn::KINBWD_WREBWD
@ KINBWD_WREBWD
Definition: iDynInv.h:69
iCub::iDyn::KINFWD_WREFWD
@ KINFWD_WREFWD
Definition: iDynInv.h:69
iCub::iDyn::asForceMoment
bool asForceMoment(const yarp::sig::Vector &w, yarp::sig::Vector &f, yarp::sig::Vector &m)
iCub::iDyn::DYNAMIC
@ DYNAMIC
Definition: iDynInv.h:64
iCub::iDyn::STATIC
@ STATIC
Definition: iDynInv.h:64
iCub::iDyn::ChainIterationMode
ChainIterationMode
Definition: iDynInv.h:68
iCub::iDyn::BACKWARD
@ BACKWARD
Definition: iDynInv.h:68
iCub::iDyn::FORWARD
@ FORWARD
Definition: iDynInv.h:68
iCub::iDyn::NewEulMode_s
const std::string NewEulMode_s[4]
Definition: iDynInv.h:65
iCub::iDyn::workInProgress
void workInProgress(const unsigned int verbose, const std::string &msg)
Definition: iDyn.cpp:64
iCub::iDyn::asWrench
bool asWrench(yarp::sig::Vector &w, const yarp::sig::Vector &f, const yarp::sig::Vector &m)
strain::dsp::fsc::max
const FSC max
Definition: strain.h:48
strain::dsp::fsc::min
const FSC min
Definition: strain.h:49
A
A
Definition: sine.m:16
offset
degrees offset
Definition: sine.m:4
Generated on Wed Nov 13 2024 21:11:41 for iCub-main by doxygen 1.9.1