iCub-main
iDynInv.cpp
Go to the documentation of this file.
1 /*
2 * Copyright (C) 2010 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 <iostream>
20 #include <yarp/math/Math.h>
21 #include <iCub/ctrl/math.h>
22 #include <iCub/iKin/iKinFwd.h>
23 #include <iCub/iDyn/iDyn.h>
24 #include <iCub/iDyn/iDynInv.h>
25 #include <stdio.h>
26 #include <deque>
27 #include <string>
28 #include <sstream> // for debug
29 
30 using namespace std;
31 using namespace yarp::sig;
32 using namespace yarp::math;
33 using namespace iCub::ctrl;
34 using namespace iCub::iKin;
35 using namespace iCub::iDyn;
36 using namespace iCub::skinDynLib;
37 
38 
39 //================================
40 //
41 // ONE LINK NEWTON EULER
42 //
43 //================================
44 
45 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
46 OneLinkNewtonEuler::OneLinkNewtonEuler(iDynLink *dlink)
47 {
48  mode = DYNAMIC;
49  verbose = NO_VERBOSE;
50  info = "link";
51  link = dlink;
52  z0.resize(3); z0.zero(); z0(2)=1;
53  zm.resize(3); zm.zero();
54 }
55 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
56 OneLinkNewtonEuler::OneLinkNewtonEuler(const NewEulMode _mode, unsigned int verb, iDynLink *dlink)
57 {
58  mode = _mode;
59  verbose = verb;
60  info = "link";
61  link = dlink;
62  z0.resize(3); z0.zero(); z0(2)=1;
63  zm.resize(3); zm.zero();
64 }
65 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
66 void OneLinkNewtonEuler::zero()
67 {
68  z0.resize(3); z0.zero(); z0(2)=1;
69  zm.resize(3); zm.zero();
70  if (link != NULL) link->zero();
71 }
72 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
73 
74  //~~~~~~~~~~~~~~~~~~~~~~
75  // set methods
76  //~~~~~~~~~~~~~~~~~~~~~~
77 
78 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
79 bool OneLinkNewtonEuler::setAsBase(const Vector &_w, const Vector &_dw, const Vector &_ddp)
80 {
81  zero();
82  setAngVel(_w);
83  setAngAcc(_dw);
84  setLinAcc(_ddp);
85  info = "base";
86  return true;
87 }
88 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
89 bool OneLinkNewtonEuler::setAsBase(const Vector &_F, const Vector &_Mu)
90 {
91  zero();
92  setForce(_F);
93  setMoment(_Mu);
94  info = "base";
95  return true;
96 }
97 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
98 bool OneLinkNewtonEuler::setAsFinal(const Vector &_F, const Vector &_Mu)
99 {
100  zero();
101  setForce(_F);
102  setMoment(_Mu);
103  info = "final";
104  return true;
105 }
106 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
107 bool OneLinkNewtonEuler::setAsFinal(const Vector &_w, const Vector &_dw, const Vector &_ddp)
108 {
109  zero();
110  setAngVel(_w);
111  setAngAcc(_dw);
112  setLinAcc(_ddp);
113  info = "final";
114  return true;
115 }
116 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
117 void OneLinkNewtonEuler::setVerbose(unsigned int verb)
118 {
119  verbose = verb;
120 }
121 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
122 void OneLinkNewtonEuler::setMode(const NewEulMode _mode)
123 {
124  mode = _mode;
125 }
126 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
127 bool OneLinkNewtonEuler::setZM(const Vector &_zm)
128 {
129  if(_zm.length()==3)
130  {
131  zm = _zm;
132  return true;
133  }
134  else
135  {
136  if(verbose)
137  {
138  fprintf(stderr,"OneLinkNewtonEuler error: could not set Zm due to wrong size vector: ");
139  fprintf(stderr,"%d instead of 3. \n",(int)_zm.length());
140 
141  }
142  return false;
143  }
144 }
145 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
146 bool OneLinkNewtonEuler::setForce(const Vector &_F)
147 {
148  if(_F.length()==3)
149  {
150  link->F = _F;
151  return true;
152  }
153  else
154  {
155  if(verbose) fprintf(stderr,"OneLink error, could not set force due to wrong size: %d instead of 3.\n",(int)_F.length());
156 
157  return false;
158  }
159 }
160 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
161 bool OneLinkNewtonEuler::setMoment(const Vector &_Mu)
162 {
163  if(_Mu.length()==3)
164  {
165  link->Mu = _Mu;
166  return true;
167  }
168  else
169  {
170  if(verbose) fprintf(stderr,"OneLink error, could not set moment due to wrong size: %d instead of 3.\n",(int)_Mu.length());
171 
172  return false;
173  }
174 }
175 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
176 void OneLinkNewtonEuler::setTorque(const double _Tau)
177 {
178  link->Tau = _Tau;
179 }
180 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
181 bool OneLinkNewtonEuler::setAngVel(const Vector &_w)
182 {
183  if(_w.length()==3)
184  {
185  link->w = _w;
186  return true;
187  }
188  else
189  {
190  if(verbose) fprintf(stderr,"OneLink error, could not set w due to wrong size: %d instead of 3. \n",(int)_w.length());
191 
192  return false;
193  }
194 }
195 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
196 bool OneLinkNewtonEuler::setAngAcc(const Vector &_dw)
197 {
198  if(_dw.length()==3)
199  {
200  link->dw = _dw;
201  return true;
202  }
203  else
204  {
205  if(verbose) fprintf(stderr,"OneLink error, could not set dw due to wrong size: %d instead of 3. \n",(int)_dw.length());
206 
207  return false;
208  }
209 }
210 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
211 bool OneLinkNewtonEuler::setLinAcc(const Vector &_ddp)
212 {
213  if(_ddp.length()==3)
214  {
215  link->ddp = _ddp;
216  return true;
217  }
218  else
219  {
220  if(verbose) fprintf(stderr,"OneLink error, could not set ddp due to wrong size: %d instead of 3. \n",(int)_ddp.length());
221 
222  return false;
223  }
224 }
225 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
226 bool OneLinkNewtonEuler::setLinAccC(const Vector &_ddpC)
227 {
228  if(_ddpC.length()==3)
229  {
230  link->ddpC = _ddpC;
231  return true;
232  }
233  else
234  {
235  if(verbose) fprintf(stderr,"OneLink error, could not set ddpC due to wrong size: %d instead of 3. \n",(int)_ddpC.length());
236 
237  return false;
238  }
239 }
240 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
241 bool OneLinkNewtonEuler::setAngAccM(const Vector &_dwM)
242 {
243  if(_dwM.length()==3)
244  {
245  link->dwM = _dwM;
246  return true;
247  }
248  else
249  {
250  if(verbose) fprintf(stderr,"OneLink error, could not set dwM due to wrong size: %d instead of 3. \n",(int)_dwM.length());
251 
252  return false;
253  }
254 }
255 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
256 void OneLinkNewtonEuler::setInfo(const string &_info)
257 {
258  info = _info;
259 }
260 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
261 bool OneLinkNewtonEuler::setMeasuredFMu(const Vector &_F, const Vector &_Mu)
262 {
263  if(link != NULL)
264  {
265  if((_F.length()==3)&&(_Mu.length()==3))
266  {
267  link->setForceMoment(_F,_Mu);
268  return true;
269  }
270  else
271  {
272  if(verbose) fprintf(stderr,"OneLinkNewtonEuler error: could not set forces/moments due to wrong dimensions: (%d,%d) instead of (3,3). \n",(int)_F.length(),(int)_Mu.length());
273 
274  return false;
275  }
276  }
277  else
278  {
279  if(verbose) fprintf(stderr,"OneLinkNewtonEuler error: could not set forces/moments due to missing link. \n");
280  return false;
281  }
282 
283 
284 }
285 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
286 bool OneLinkNewtonEuler::setMeasuredTorque(const double _Tau)
287 {
288  if(link != NULL)
289  {
290  link->setTorque(_Tau);
291  return true;
292  }
293  else
294  {
295  if(verbose) fprintf(stderr,"OneLinkNewtonEuler error: could not set torque due to missing link. \n");
296  return false;
297  }
298 
299 }
300 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
301 string OneLinkNewtonEuler::toString() const
302 {
303  string ret = "[Link/frame]: " + info + " [Mode]: " + NewEulMode_s[mode];
304 
305  char buffer[300]; int j=0;
306  j=sprintf(buffer," [Torque]: %f",link->Tau);
307  j+=sprintf(buffer+j," [Force]: %.3f,%.3f,%.3f",link->F(0),link->F(1),link->F(2));
308  j+=sprintf(buffer+j," [Moment]: %.3f,%.3f,%.3f",link->Mu(0),link->Mu(1),link->Mu(2));
309  if(verbose)
310  {
311  j+=sprintf(buffer+j," [mass]: %.3f",link->m);
312  Vector r = link->getr();
313  j+=sprintf(buffer+j," [r]: %.3f,%.3f,%.3f",r(0),r(1),r(2));
314  r = link->getrC();
315  j+=sprintf(buffer+j," [rC]: %.3f,%.3f,%.3f",r(0),r(1),r(2));
316 
317  if(mode!=STATIC)
318  {
319  Matrix I = link->getInertia();
320  j+=sprintf(buffer+j," [Inertia]: %.3f, %.3f, %.3f; %.3f, %.3f, %.3f; %.3f, %.3f, %.3f",I(0,0),I(0,1),I(0,2),I(1,0),I(1,1),I(1,2),I(2,0),I(2,1),I(2,2));
321  }
322  }
323  ret.append(buffer);
324  return ret;
325 }
326 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
327 
328 
329  //~~~~~~~~~~~~~~~~~~~~~~
330  // get methods
331  //~~~~~~~~~~~~~~~~~~~~~~
332 
333 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
334 NewEulMode OneLinkNewtonEuler::getMode() const { return mode;}
335 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
336 Vector OneLinkNewtonEuler::getZM() const { return zm;}
337 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
338 const Vector& OneLinkNewtonEuler::getAngVel() const { return link->w;}
339 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
340 const Vector& OneLinkNewtonEuler::getAngAcc() const { return link->dw;}
341 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
342 const Vector& OneLinkNewtonEuler::getAngAccM() const { return link->dwM;}
343 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
344 const Vector& OneLinkNewtonEuler::getLinAcc() const { return link->ddp;}
345 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
346 const Vector& OneLinkNewtonEuler::getLinAccC() const { return link->ddpC;}
347 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
348 const Vector& OneLinkNewtonEuler::getForce() const { return link->F;}
349 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
350 const Vector& OneLinkNewtonEuler::getMoment(bool isBase) const { return link->Mu;}
351 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
352 double OneLinkNewtonEuler::getTorque() const { return link->Tau;}
353 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
354 string OneLinkNewtonEuler::getInfo() const { return info;}
355 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
356 const Matrix& OneLinkNewtonEuler::getH() { return link->getH();}
357 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
358 const Matrix& OneLinkNewtonEuler::getR() { return link->getR();}
359 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
360 const Matrix& OneLinkNewtonEuler::getRC() { return link->getRC();}
361 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
362 double OneLinkNewtonEuler::getIm() const { return link->Im;}
363 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
364 double OneLinkNewtonEuler::getD2q() const { return link->ddq;}
365 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
366 double OneLinkNewtonEuler::getDq() const { return link->dq;}
367 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
368 double OneLinkNewtonEuler::getKr() const { return link->kr;}
369 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
370 double OneLinkNewtonEuler::getFs() const { return link->Fs;}
371 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
372 double OneLinkNewtonEuler::getFv() const { return link->Fv;}
373 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
374 double OneLinkNewtonEuler::getMass()const { return link->m;}
375 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
376 const Matrix& OneLinkNewtonEuler::getInertia()const { return link->getInertia();}
377 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
378 const Vector& OneLinkNewtonEuler::getr(bool proj) { return link->getr(proj);}
379 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
380 const Vector& OneLinkNewtonEuler::getrC(bool proj) { return link->getrC(proj);}
381 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
382 
383  //~~~~~~~~~~~~~~~~~~~~~~
384  // core computation
385  //~~~~~~~~~~~~~~~~~~~~~~
386 
387 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
388 void OneLinkNewtonEuler::computeAngVel(OneLinkNewtonEuler *prev)
389 {
390  switch(mode)
391  {
392  case DYNAMIC_CORIOLIS_GRAVITY:
393  case DYNAMIC:
394  case DYNAMIC_W_ROTOR:
395  {
396  Vector w = prev->getAngVel();
397  w[2] += getDq();
398  setAngVel(w*getR());
399  //setAngVel( getR().transposed() * ( prev->getAngVel() + getDq() * z0 ));
400  break;
401  }
402  case STATIC:
403  setAngVel(zeros(3));
404  break;
405  }
406 }
407 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
408 void OneLinkNewtonEuler::computeAngVelBackward(OneLinkNewtonEuler *next)
409 {
410  switch(mode)
411  {
412  case DYNAMIC_CORIOLIS_GRAVITY:
413  case DYNAMIC:
414  case DYNAMIC_W_ROTOR:
415  {
416  Vector w = next->getR() * next->getAngVel();
417  w[2] -= next->getDq();
418  setAngVel(w);
419  //setAngVel( next->getR() * next->getAngVel() - next->getDq() * z0 );
420  break;
421  }
422  case STATIC:
423  setAngVel(zeros(3));
424  break;
425  }
426 }
427 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
428 void OneLinkNewtonEuler::computeAngAcc(OneLinkNewtonEuler *prev)
429 {
430  switch(mode)
431  {
432  case DYNAMIC:
433  case DYNAMIC_W_ROTOR:
434  {
435  Vector dw = prev->getAngAcc();
436  const Vector& prevW = prev->getAngVel();
437  dw[0] += getDq()*prevW[1];
438  dw[1] -= getDq()*prevW[0];
439  dw[2] += getD2q();
440  setAngAcc(dw * getR());
441  //setAngAcc( (getR()).transposed() * ( prev->getAngAcc() + getD2q()*z0 + getDq() * cross(prev->getAngVel(),z0));
442  break;
443  }
444  case DYNAMIC_CORIOLIS_GRAVITY:
445  {
446  Vector dw = prev->getAngAcc();
447  const Vector& prevW = prev->getAngVel();
448  dw[0] += getDq()*prevW[1];
449  dw[1] -= getDq()*prevW[0];
450  setAngAcc(dw * getR());
451  //setAngAcc( (getR()).transposed() * ( prev->getAngAcc() + getDq() * cross(prev->getAngVel(),z0) ));
452  break;
453  }
454  case STATIC:
455  setAngAcc(zeros(3));
456  break;
457  }
458 }
459 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
460 void OneLinkNewtonEuler::computeAngAccBackward(OneLinkNewtonEuler *next)
461 {
462  switch(mode)
463  {
464  case DYNAMIC:
465  case DYNAMIC_W_ROTOR:
466  {
467  Vector nextDw = next->getR() * next->getAngAcc();
468  const Vector& w = getAngVel();
469  nextDw[0] -= next->getDq()*w[1];
470  nextDw[1] += next->getDq()*w[0];
471  nextDw[2] -= next->getD2q();
472  setAngAcc(nextDw);
473  //setAngAcc( next->getR() * next->getAngAcc() - next->getD2q() * z0 - next->getDq() * cross(getAngVel(),z0) );
474  break;
475  }
476  case DYNAMIC_CORIOLIS_GRAVITY:
477  {
478  Vector nextDw = next->getR() * next->getAngAcc();
479  const Vector& w = getAngVel();
480  nextDw[0] -= next->getDq()*w[1];
481  nextDw[1] += next->getDq()*w[0];
482  setAngAcc(nextDw);
483  //setAngAcc( next->getR() * next->getAngAcc() - next->getDq() * cross(getAngVel(),z0) );
484  break;
485  }
486  case STATIC:
487  setAngAcc(zeros(3));
488  break;
489  }
490 }
491 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
492 void OneLinkNewtonEuler::computeLinAcc(OneLinkNewtonEuler *prev)
493 {
494  const Matrix& R = getR();
495  switch(mode)
496  {
497  case DYNAMIC:
498  case DYNAMIC_CORIOLIS_GRAVITY:
499  case DYNAMIC_W_ROTOR:
500  {
501  const Vector& r = getr(true);
502  Vector temp = prev->getLinAcc()*R;
503  temp += cross(link->dw, r);
504  temp += cross(link->w, cross(link->w, r));
505  setLinAcc(temp);
506  /*setLinAcc( prev->getLinAcc()*R
507  + cross(getAngAcc(), r)
508  + cross(getAngVel(), cross(getAngVel(), r)) );*/
509  break;
510  }
511  case STATIC:
512  setLinAcc( prev->getLinAcc()*R );
513  break;
514  }
515 }
516 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
517 void OneLinkNewtonEuler::computeLinAccBackward(OneLinkNewtonEuler *next)
518 {
519  const Matrix& R = next->getR();
520  switch(mode)
521  {
522  case DYNAMIC:
523  case DYNAMIC_CORIOLIS_GRAVITY:
524  case DYNAMIC_W_ROTOR:
525  {
526  const Vector& r = next->getr(true);
527  Vector temp = next->getLinAcc();
528  temp -= cross(next->getAngAcc(), r);
529  temp -= cross(next->getAngVel(), cross(next->getAngVel(), r));
530  setLinAcc(R*temp);
531  /*setLinAcc(R * (next->getLinAcc()
532  - cross(next->getAngAcc(), r)
533  - cross(next->getAngVel(), cross(next->getAngVel(), r)) ));*/
534  break;
535  }
536  case STATIC:
537  setLinAcc( R * next->getLinAcc() );
538  break;
539  }
540 }
541 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
542 void OneLinkNewtonEuler::computeLinAccC()
543 {
544  switch(mode)
545  {
546  case DYNAMIC:
547  case DYNAMIC_CORIOLIS_GRAVITY:
548  case DYNAMIC_W_ROTOR:
549  {
550  Vector temp = getLinAcc();
551  temp += cross(getAngAcc(),getrC());
552  temp += cross(getAngVel(),cross(getAngVel(),getrC()));
553  setLinAccC(temp);
554  //setLinAccC( getLinAcc() + cross(getAngAcc(),getrC()) + cross(getAngVel(),cross(getAngVel(),getrC())));
555  break;
556  }
557  case STATIC:
558  setLinAccC( getLinAcc());
559  break;
560  }
561 }
562 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
563 void OneLinkNewtonEuler::computeAngAccM(OneLinkNewtonEuler *prev)
564 {
565  switch(mode)
566  {
567  case DYNAMIC_CORIOLIS_GRAVITY:
568  case DYNAMIC:
569  setAngAccM( prev->getAngAcc());
570  break;
571  case DYNAMIC_W_ROTOR:
572  setAngAccM( prev->getAngAcc() + (getKr() * getD2q()) * zm
573  + (getKr() * getDq()) * cross(prev->getAngVel(),zm) );
574  break;
575  case STATIC:
576  setAngAccM(zeros(3));
577  break;
578  }
579 }
580 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
581 void OneLinkNewtonEuler::computeForceBackward(OneLinkNewtonEuler *next)
582 {
583  Vector temp = next->getMass() * next->getLinAccC();
584  temp += next->getForce();
585  setForce(next->getR() * temp);
586 }
587 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
588 void OneLinkNewtonEuler::computeForceForward(OneLinkNewtonEuler *prev)
589 {
590  Vector temp = prev->getForce()*getR();
591  temp -= getMass() * getLinAccC();
592  setForce(temp);
593  //setForce( prev->getForce()*getR() - getMass() * getLinAccC() );
594 }
595 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
596 void OneLinkNewtonEuler::computeMomentBackward(OneLinkNewtonEuler *next)
597 {
598  const Matrix& Rn = next->getR();
599  const Vector& rnp = next->getr(true);
600  switch(mode)
601  {
602  case DYNAMIC_CORIOLIS_GRAVITY:
603  case DYNAMIC:
604  {
605  Vector temp = cross(rnp , next->getForce());
606  temp += cross(rnp + next->getrC() , next->getMass() * next->getLinAccC());
607  temp += next->getMoment(false);
608  temp += next->getInertia() * next->getAngAcc();
609  temp += cross( next->getAngVel() , next->getInertia() * next->getAngVel());
610  setMoment(Rn*temp);
611  /*setMoment( Rn * ( cross(rnp , next->getForce())
612  + cross(rnp + next->getrC() , next->getMass() * next->getLinAccC())
613  + next->getMoment(false)
614  + next->getInertia() * next->getAngAcc()
615  + cross( next->getAngVel() , next->getInertia() * next->getAngVel()))
616  );*/
617  break;
618  }
619  case DYNAMIC_W_ROTOR:
620  setMoment( Rn * ( cross(rnp , next->getForce())
621  + cross(rnp + next->getrC() , next->getMass() * next->getLinAccC())
622  + next->getMoment(false)
623  + next->getInertia() * next->getAngAcc()
624  + cross( next->getAngVel() , next->getInertia() * next->getAngVel()))
625  + next->getKr() * next->getD2q() * next->getIm() * next->getZM()
626  + next->getKr() * next->getDq() * next->getIm() * cross(next->getAngVel(),next->getZM()) );
627  break;
628  case STATIC:
629  {
630  Vector temp = cross(rnp , next->getForce());
631  temp += cross(rnp + next->getrC() , next->getMass() * next->getLinAccC());
632  temp += next->getMoment(false);
633  setMoment(Rn*temp);
634  /*setMoment( Rn * ( cross(rnp , next->getForce())
635  + cross(rnp + next->getrC() , next->getMass() * next->getLinAccC())
636  + next->getMoment(false)));*/
637  break;
638  }
639  }
640 }
641 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
642 void OneLinkNewtonEuler::computeMomentForward( OneLinkNewtonEuler *prev)
643 {
644  const Matrix& R = link->getR();
645  const Vector& RTr = link->getr(true);
646  switch(mode)
647  {
648  case DYNAMIC_CORIOLIS_GRAVITY:
649  case DYNAMIC:
650  {
651  Vector temp = prev->getMoment(false)*R;
652  temp -= cross(RTr, link->F);
653  temp -= cross(RTr + link->rc, link->m * link->ddpC);
654  temp -= link->I * link->dw;
655  temp -= cross( link->w, link->I * link->w);
656  setMoment(temp);
657  /*setMoment( prev->getMoment(false)*R- cross(RTr, getForce())
658  - cross(RTr + getrC(), getMass() * getLinAccC())
659  - getInertia() * getAngAcc()
660  - cross( getAngVel(), getInertia()*getAngVel())
661  );*/
662  break;
663  }
664  case DYNAMIC_W_ROTOR:
665  {
666  Vector temp = prev->getMoment(false);
667  temp -= getKr() * getD2q() * getIm() * getZM();
668  temp -= getKr() * getDq() * getIm() * cross(getAngVel(),getZM());
669  temp *= R;
670  temp -= cross(RTr, getForce());
671  temp -= cross(RTr + getrC(), getMass() * getLinAccC());
672  temp -= getInertia() * getAngAcc();
673  temp -= cross( getAngVel(), getInertia()*getAngVel());
674  setMoment(temp);
675  /*setMoment( (prev->getMoment(false)
676  - getKr() * getD2q() * getIm() * getZM()
677  - getKr() * getDq() * getIm() * cross(getAngVel(),getZM()))*R
678  - cross(RTr, getForce())
679  - cross(RTr + getrC(), getMass() * getLinAccC())
680  - getInertia() * getAngAcc()
681  - cross( getAngVel(), getInertia()*getAngVel())
682  );*/
683  break;
684  }
685  case STATIC:
686  {
687  Vector temp = prev->getMoment(false)*R;
688  temp -= cross(RTr, getForce());
689  temp -= cross(RTr + getrC(), getMass() * getLinAccC());
690  setMoment(temp);
691  /*setMoment( prev->getMoment(false)*R - cross(RTr, getForce())
692  - cross(RTr + getrC(), getMass() * getLinAccC()));*/
693  break;
694  }
695  }
696 }
697 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
698 void OneLinkNewtonEuler::computeTorque(OneLinkNewtonEuler *prev)
699 {
700  switch(mode)
701  {
702  case DYNAMIC_CORIOLIS_GRAVITY:
703  case DYNAMIC:
704  case STATIC:
705  setTorque(prev->getMoment(true)[2]);
706  break;
707  case DYNAMIC_W_ROTOR:
708  setTorque(prev->getMoment(true)[2] + getKr() * getIm() * dot(getAngAccM(),zm)
709  + getFv() * getDq() + getFs() * sign(getDq()));
710  break;
711  }
712 }
713 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
714 
715  //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
716  // main computation methods
717  //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
718 
719 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
720 void OneLinkNewtonEuler::ForwardKinematics( OneLinkNewtonEuler *prev)
721 {
722  this->computeAngVel(prev);
723  this->computeAngAcc(prev);
724  this->computeLinAcc(prev);
725  this->computeLinAccC();
726  //this->computeAngAccM(prev);
727 }
728 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
729 void OneLinkNewtonEuler::BackwardKinematics( OneLinkNewtonEuler *prev)
730 {
731  this->computeAngVelBackward(prev);
732  this->computeAngAccBackward(prev);
733  this->computeLinAccBackward(prev);
734  this->computeLinAccC();
735 }
736 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
737 void OneLinkNewtonEuler::BackwardWrench( OneLinkNewtonEuler *next)
738 {
739  this->computeForceBackward(next);
740  this->computeMomentBackward(next);
741 }
742 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
743 void OneLinkNewtonEuler::ForwardWrench( OneLinkNewtonEuler *prev)
744 {
745  this->computeForceForward(prev);
746  this->computeMomentForward(prev);
747  this->computeTorque(prev);
748 }
749 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
750 
751 
752 //================================
753 //
754 // BASE LINK NEWTON EULER
755 //
756 //================================
757 
758 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
759 BaseLinkNewtonEuler::BaseLinkNewtonEuler(const Matrix &_H0, const NewEulMode _mode, unsigned int verb)
760 : OneLinkNewtonEuler(_mode,verb,NULL), eye3x3(eye(3,3)), zeros3x3(zeros(3,3)), zeros3(zeros(3))
761 {
762  info = "base";
763  w.resize(3); w.zero();
764  dw.resize(3); dw.zero();
765  ddp.resize(3); ddp.zero();
766  H0.resize(4,4); H0.eye();
767  if((_H0.rows()==4)&&(_H0.cols()==4))
768  H0 = _H0;
769  else
770  if(verbose)
771  {
772  fprintf(stderr,"BaseLink error, could not set H0 due to wrong dimensions: ( %zu,%zu) instead of (4,4). \n",_H0.rows(),_H0.cols());
773 
774  fprintf(stderr," Default is set. \n");
775  }
776  F.resize(3); F.zero();
777  Mu.resize(3); Mu.zero();
778  Tau = 0.0;
779 }
780 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
781 BaseLinkNewtonEuler::BaseLinkNewtonEuler(const Matrix &_H0, const Vector &_w, const Vector &_dw, const Vector &_ddp, const NewEulMode _mode, unsigned int verb)
782 : OneLinkNewtonEuler(_mode,verb,NULL), eye3x3(eye(3,3)), zeros3x3(zeros(3,3)), zeros3(zeros(3))
783 {
784  info = "base";
785  w.resize(3); w.zero();
786  dw.resize(3); dw.zero();
787  ddp.resize(3); ddp.zero();
788  H0.resize(4,4); H0.eye();
789  if((_H0.rows()==4)&&(_H0.cols()==4))
790  H0 = _H0;
791  else
792  if(verbose)
793  {
794  fprintf(stderr,"BaseLink error, could not set H0 due to wrong dimensions: ( %zu,%zu) instead of (4,4). \n",_H0.rows(),_H0.cols());
795 
796  fprintf(stderr," Default is set. \n");
797  }
798  F.resize(3); F.zero();
799  Mu.resize(3); Mu.zero();
800  Tau = 0.0;
801  setAsBase(_w,_dw,_ddp);
802 }
803 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
804 bool BaseLinkNewtonEuler::setAsBase(const Vector &_w, const Vector &_dw, const Vector &_ddp)
805 {
806  if((_w.length()==3)&&(_dw.length()==3)&&(_ddp.length()==3))
807  {
808  this->setAngVel(_w);
809  this->setAngAcc(_dw);
810  this->setLinAcc(_ddp);
811  return true;
812  }
813  else
814  {
815  w.resize(3); w.zero();
816  dw.resize(3); dw.zero();
817  ddp.resize(3); ddp.zero();
818 
819  if(verbose)
820  {
821  fprintf(stderr,"BaseLinkNewtonEuler error: could not set w/dw/ddp due to wrong dimensions: (%d,%d,%d) instead of (3,3,3). ",(int)_w.length(),(int)_dw.length(),(int)_ddp.length());
822 
823  fprintf(stderr," Default is set. \n");
824  }
825  return false;
826  }
827 }
828 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
829 bool BaseLinkNewtonEuler::setAsBase(const Vector &_F, const Vector &_Mu)
830 {
831  if((_F.length()==3)&&(_Mu.length()==3))
832  {
833  F = _F;
834  Mu = _Mu;
835  return true;
836  }
837  else
838  {
839  F.resize(3); F.zero();
840  Mu.resize(3); Mu.zero();
841 
842  if(verbose)
843  {
844  fprintf(stderr,"FinalLinkNewtonEuler error: could not set F/Mu due to wrong dimensions: (%d,%d) instead of (3,3).",(int)_F.length(),(int)_Mu.length());
845 
846  fprintf(stderr," Default is set. \n");
847  }
848  return false;
849  }
850 }
851 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
852 string BaseLinkNewtonEuler::toString() const
853 {
854  string ret = "[Base link]: " + info + " [Mode]: " + NewEulMode_s[mode];
855 
856  char buffer[300]; int j=0;
857  if(verbose)
858  {
859  j=sprintf(buffer," [w] : %.3f,%.3f,%.3f",w(0),w(1),w(2));
860  j+=sprintf(buffer+j," [dw] : %.3f,%.3f,%.3f",dw(0),dw(1),dw(2));
861  j+=sprintf(buffer+j," [ddp]: %.3f,%.3f,%.3f",ddp(0),ddp(1),ddp(2));
862  }
863  ret.append(buffer);
864  return ret;
865 }
866 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
867 
868  //~~~~~~~~~~~~~~~~~~~~~~
869  // get methods
870  //~~~~~~~~~~~~~~~~~~~~~~
871 
872 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
873 const Vector& BaseLinkNewtonEuler::getAngVel() const {return w;}
874 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
875 const Vector& BaseLinkNewtonEuler::getAngAcc() const {return dw;}
876 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
877 const Vector& BaseLinkNewtonEuler::getAngAccM() const {return dw;}
878 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
879 const Vector& BaseLinkNewtonEuler::getLinAcc() const {return ddp; }
880 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
881 const Vector& BaseLinkNewtonEuler::getLinAccC() const {return getLinAcc();}
882 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
883 const Vector& BaseLinkNewtonEuler::getForce() const {return F;}
884 const Vector& BaseLinkNewtonEuler::getMoment(bool isBase) const
885 {
886  if(isBase==false)
887  return Mu;
888  return Mu0;
889 }
890 double BaseLinkNewtonEuler::getTorque()const {return Tau;}
891 const Matrix& BaseLinkNewtonEuler::getR() {return eye3x3;}
892 const Matrix& BaseLinkNewtonEuler::getRC() {return eye3x3;}
893 double BaseLinkNewtonEuler::getIm() const {return 0.0;}
894 double BaseLinkNewtonEuler::getFs() const {return 0.0;}
895 double BaseLinkNewtonEuler::getFv() const {return 0.0;}
896 double BaseLinkNewtonEuler::getD2q() const {return 0.0;}
897 double BaseLinkNewtonEuler::getDq() const {return 0.0;}
898 double BaseLinkNewtonEuler::getKr() const {return 0.0;}
899 double BaseLinkNewtonEuler::getMass() const {return 0.0;}
900 const Matrix& BaseLinkNewtonEuler::getInertia()const {return zeros3x3;}
901 const Vector& BaseLinkNewtonEuler::getr(bool proj) {return zeros3;}
902 const Vector& BaseLinkNewtonEuler::getrC(bool proj){return zeros3;}
903 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
904 bool BaseLinkNewtonEuler::setForce(const Vector &_F)
905 {
906  if(_F.length()==3)
907  {
908  F=H0.submatrix(0,2,0,2)*_F;
909  return true;
910  }
911  else
912  {
913  if(verbose)
914  fprintf(stderr,"BaseLink error, could not set force due to wrong dimension: %d instead of 3.\n",(int)_F.length());
915 
916  return false;
917  }
918 }
919 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
920 bool BaseLinkNewtonEuler::setMoment(const Vector &_Mu)
921 {
922  if(_Mu.length()==3)
923  {
924  Mu = H0.submatrix(0,2,0,2)*_Mu;
925  Mu0 = _Mu;
926  return true;
927  }
928  else
929  {
930  if(verbose)
931  fprintf(stderr,"BaseLink error, could not set moment due to wrong dimension: %d instead of 3.\n",(int)_Mu.length());
932 
933  return false;
934  }
935 }
936 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
937 void BaseLinkNewtonEuler::setTorque(const double _Tau) {Tau=_Tau;}
938 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
939 bool BaseLinkNewtonEuler::setAngVel(const Vector &_w)
940 {
941  if(_w.length()==3)
942  {
943  w = H0.submatrix(0,2,0,2).transposed()*_w;
944  return true;
945  }
946  else
947  {
948  if(verbose)
949  fprintf(stderr,"BaseLink error, could not set w due to wrong size: %d instead of 3. \n",(int)_w.length());
950 
951  return false;
952  }
953 }
954 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
955 bool BaseLinkNewtonEuler::setAngAcc(const Vector &_dw)
956 {
957  if(_dw.length()==3)
958  {
959  dw = H0.submatrix(0,2,0,2).transposed()*_dw;
960  return true;
961  }
962  else
963  {
964  if(verbose)
965  fprintf(stderr,"BaseLink error, could not set dw due to wrong size: %d instead of 3. \n",(int)_dw.length());
966 
967  return false;
968  }
969 }
970 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
971 bool BaseLinkNewtonEuler::setLinAcc(const Vector &_ddp)
972 {
973  if(_ddp.length()==3)
974  {
975  ddp = H0.submatrix(0,2,0,2).transposed()*_ddp;
976  return true;
977  }
978  else
979  {
980  if(verbose)
981  fprintf(stderr,"BaseLink error, could not set ddp due to wrong size: %d instead of 3. \n",(int)_ddp.length());
982 
983  return false;
984  }
985 }
986 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
987 bool BaseLinkNewtonEuler::setLinAccC(const Vector &_ddpC)
988 {
989  if(verbose)
990  fprintf(stderr,"BaseLink error: no ddpC existing \n");
991  return false;
992 }
993 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
994 bool BaseLinkNewtonEuler::setAngAccM(const Vector &_dwM)
995 {
996  if(verbose)
997  fprintf(stderr,"BaseLink error: no dwM existing \n");
998  return false;
999 }
1000 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
1001 
1002 
1003 
1004 //================================
1005 //
1006 // FINAL LINK NEWTON EULER
1007 //
1008 //================================
1009 
1010 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
1011 FinalLinkNewtonEuler::FinalLinkNewtonEuler(const yarp::sig::Matrix &_HN, const NewEulMode _mode, unsigned int verb)
1012 : OneLinkNewtonEuler(_mode,verb,NULL), eye4x4(eye(4,4)), eye3x3(eye(3,3)), zeros3x3(zeros(3,3)), zeros3(zeros(3))
1013 {
1014  info = "final";
1015  F.resize(3); F.zero();
1016  Mu.resize(3); Mu.zero();
1017  w.resize(3); w.zero();
1018  dw.resize(3); dw.zero();
1019  ddp.resize(3); ddp.zero();
1020  HN.resize(4,4); HN.eye();
1021  if((_HN.rows()==4)&&(_HN.cols()==4))
1022  HN = _HN;
1023  else
1024  if(verbose)
1025  {
1026  fprintf(stderr,"BaseLink error, could not set HN due to wrong dimensions: ( %zu,%zu) instead of (4,4). \n",_HN.rows(),_HN.cols());
1027 
1028  fprintf(stderr," Default is set. \n");
1029  }
1030 }
1031 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
1032 FinalLinkNewtonEuler::FinalLinkNewtonEuler(const yarp::sig::Matrix &_HN, const Vector &_F, const Vector &_Mu, const NewEulMode _mode, unsigned int verb)
1033 : OneLinkNewtonEuler(_mode,verb,NULL), eye4x4(eye(4,4)), eye3x3(eye(3,3)), zeros3x3(zeros(3,3)), zeros3(zeros(3))
1034 {
1035  info = "final";
1036  F.resize(3); F.zero();
1037  Mu.resize(3); Mu.zero();
1038  w.resize(3); w.zero();
1039  dw.resize(3); dw.zero();
1040  ddp.resize(3); ddp.zero();
1041  setAsFinal(_F,_Mu);
1042  HN.resize(4,4); HN.eye();
1043  if((_HN.rows()==4)&&(_HN.cols()==4))
1044  HN = _HN;
1045  else
1046  if(verbose)
1047  {
1048  fprintf(stderr,"BaseLink error, could not set H0 due to wrong dimensions: ( %zu,%zu) instead of (4,4). \n",_HN.rows(),_HN.cols());
1049 
1050  fprintf(stderr," Default is set. \n");
1051  }
1052 }
1053 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
1054 bool FinalLinkNewtonEuler::setAsFinal(const Vector &_w, const Vector &_dw, const Vector &_ddp)
1055 {
1056  if((_w.length()==3)&&(_dw.length()==3)&&(_ddp.length()==3))
1057  {
1058  w = _w;
1059  dw = _dw;
1060  ddp = _ddp;
1061  return true;
1062  }
1063  else
1064  {
1065  w.resize(3); w.zero();
1066  dw.resize(3); dw.zero();
1067  ddp.resize(3); ddp.zero();
1068 
1069  if(verbose)
1070  {
1071  fprintf(stderr,"FinalLinkNewtonEuler error: could not set w/dw/ddp due to wrong dimensions: (%d,%d,%d) instead of (3,3,3).",(int)_w.length(),(int)_dw.length(),(int)_ddp.length());
1072 
1073  fprintf(stderr," Default is set. \n");
1074  }
1075  return false;
1076  }
1077 }
1078 
1079 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
1080 bool FinalLinkNewtonEuler::setAsFinal(const Vector &_F, const Vector &_Mu)
1081 {
1082  if((_F.length()==3)&&(_Mu.length()==3))
1083  {
1084  F = _F;
1085  Mu = _Mu;
1086  return true;
1087  }
1088  else
1089  {
1090  F.resize(3); F.zero();
1091  Mu.resize(3); Mu.zero();
1092 
1093  if(verbose)
1094  {
1095  fprintf(stderr,"FinalLinkNewtonEuler error: could not set F/Mu due to wrong dimensions: (%d,%d) instead of (3,3).",(int)_F.length(),(int)_Mu.length());
1096 
1097  fprintf(stderr," Default is set. \n");
1098  }
1099  return false;
1100  }
1101 }
1102 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
1103 string FinalLinkNewtonEuler::toString() const
1104 {
1105  string ret = "[Final link]: " + info + " [Mode]: " + NewEulMode_s[mode];
1106 
1107  char buffer[300]; int j=0;
1108  if(verbose)
1109  {
1110  j=sprintf( buffer," [F] : %.3f,%.3f,%.3f",F(0),F(1),F(2));
1111  j+=sprintf(buffer+j," [Mu] : %.3f,%.3f,%.3f",Mu(0),Mu(1),Mu(2));
1112  }
1113  ret.append(buffer);
1114  return ret;
1115 }
1116 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
1117 
1118  //~~~~~~~~~~~~~~~~~~~~~~
1119  // get methods
1120  //~~~~~~~~~~~~~~~~~~~~~~
1121 
1122 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
1123 const Vector& FinalLinkNewtonEuler::getForce() const {return F;}
1124 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
1125 const Vector& FinalLinkNewtonEuler::getMoment(bool isBase) const {return Mu;}
1126 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
1127 const Vector& FinalLinkNewtonEuler::getAngVel() const {return w;}
1128 const Vector& FinalLinkNewtonEuler::getAngAcc() const {return dw;}
1129 const Vector& FinalLinkNewtonEuler::getAngAccM() const {return zeros3;}
1130 const Vector& FinalLinkNewtonEuler::getLinAcc() const {return ddp;}
1131 const Vector& FinalLinkNewtonEuler::getLinAccC() const {return zeros3;}
1132 double FinalLinkNewtonEuler::getTorque() const {return 0.0;}
1133 const Matrix& FinalLinkNewtonEuler::getH() {return eye4x4;}
1134 const Matrix& FinalLinkNewtonEuler::getR() {return eye3x3;}
1135 const Matrix& FinalLinkNewtonEuler::getRC() {return eye3x3;}
1136 double FinalLinkNewtonEuler::getIm() const {return 0.0;}
1137 double FinalLinkNewtonEuler::getFs() const {return 0.0;}
1138 double FinalLinkNewtonEuler::getFv() const {return 0.0;}
1139 double FinalLinkNewtonEuler::getD2q() const {return 0.0;}
1140 double FinalLinkNewtonEuler::getDq() const {return 0.0;}
1141 double FinalLinkNewtonEuler::getKr() const {return 0.0;}
1142 double FinalLinkNewtonEuler::getMass() const {return 0.0;}
1143 const Matrix& FinalLinkNewtonEuler::getInertia()const {return zeros3x3;}
1144 const Vector& FinalLinkNewtonEuler::getr(bool proj) {return zeros3;}
1145 const Vector& FinalLinkNewtonEuler::getrC(bool proj) {return zeros3;}
1146 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
1147 bool FinalLinkNewtonEuler::setForce(const Vector &_F)
1148 {
1149  if(_F.length()==3)
1150  {
1151  F=_F;
1152  return true;
1153  }
1154  else
1155  {
1156  if(verbose)
1157  fprintf(stderr,"FinalLink error, could not set force due to wrong dimension: %d instead of 3.\n",(int)_F.length());
1158 
1159  return false;
1160  }
1161 }
1162 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
1163 bool FinalLinkNewtonEuler::setMoment(const Vector &_Mu)
1164 {
1165  if(_Mu.length()==3)
1166  {
1167  Mu=_Mu;
1168  return true;
1169  }
1170  else
1171  {
1172  if(verbose)
1173  fprintf(stderr,"FinalLink error, could not set moment due to wrong dimension: %d instead of 3.\n",(int)_Mu.length());
1174 
1175  return false;
1176  }
1177 }
1178 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
1179 void FinalLinkNewtonEuler::setTorque(const double _Tau){}
1180 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
1181 bool FinalLinkNewtonEuler::setAngVel(const Vector &_w)
1182 {
1183  /*if(verbose)
1184  fprintf(stderr,"FinalLink error: no w existing \n");
1185  return false;*/
1186  w=_w;
1187  return true;
1188 }
1189 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
1190 bool FinalLinkNewtonEuler::setAngAcc(const Vector &_dw)
1191 {
1192  //if(verbose)
1193  // fprintf(stderr,"FinalLink error: no dw existing \n");
1194  //return false;
1195  dw=_dw; return true;
1196 }
1197 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
1198 bool FinalLinkNewtonEuler::setLinAcc(const Vector &_ddp)
1199 {
1200  //if(verbose)
1201  // fprintf(stderr,"FinalLink error: no ddp existing \n");
1202  //return false;
1203  ddp=_ddp; return true;
1204 }
1205 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
1206 bool FinalLinkNewtonEuler::setLinAccC(const Vector &_ddpC)
1207 {
1208  if(verbose)
1209  fprintf(stderr,"FinalLink error: no ddpC existing \n");
1210  return false;
1211 }
1212 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
1213 bool FinalLinkNewtonEuler::setAngAccM(const Vector &_dwM)
1214 {
1215  if(verbose)
1216  fprintf(stderr,"FinalLink error: no dwM existing \n");
1217  return false;
1218 }
1219 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
1220 
1221 
1222 //================================
1223 //
1224 // SENSOR LINK NEWTON EULER
1225 //
1226 //================================
1227 
1228 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
1229 SensorLinkNewtonEuler::SensorLinkNewtonEuler(const NewEulMode _mode, unsigned int verb)
1230 : OneLinkNewtonEuler(_mode,verb,NULL), zeros0(0,0.0)
1231 {
1232  info = "sensor";
1233  F.resize(3); F.zero();
1234  Mu.resize(3); Mu.zero();
1235  w.resize(3); w.zero();
1236  dw.resize(3); dw.zero();
1237  ddp.resize(3); ddp.zero();
1238  ddpC.resize(3); ddpC.zero();
1239  H.resize(4,4); H.eye();
1240  COM.resize(4,4); COM.eye();
1241  R = RC = eye(3,3);
1242  r = rc = r_proj = rc_proj = zeros(3);
1243  I.resize(3,3); I.zero();
1244  m=0.0;
1245 }
1246 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
1247 SensorLinkNewtonEuler::SensorLinkNewtonEuler(const Matrix &_H, const Matrix &_COM, const double _m, const Matrix &_I, const NewEulMode _mode, unsigned int verb)
1248 : OneLinkNewtonEuler(_mode,verb,NULL), zeros0(0,0.0)
1249 {
1250  info = "sensor";
1251  F.resize(3); F.zero();
1252  Mu.resize(3); Mu.zero();
1253  w.resize(3); w.zero();
1254  dw.resize(3); dw.zero();
1255  ddp.resize(3); ddp.zero();
1256  ddpC.resize(3); ddpC.zero();
1257  setSensor(_H,_COM,_m,_I);
1258 }
1259 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
1260 bool SensorLinkNewtonEuler::setMeasuredFMu(const Vector &_F, const Vector &_Mu)
1261 {
1262  if((_F.length()==3)&&(_Mu.length()==3))
1263  {
1264  F = _F;
1265  Mu = _Mu;
1266  return true;
1267  }
1268  else
1269  {
1270  F.resize(3); F.zero();
1271  Mu.resize(3); Mu.zero();
1272 
1273  if(verbose)
1274  fprintf(stderr,"SensorLinkNewtonEuler error: could not set F/Mu due to wrong dimensions: (%d,%d) instead of (3,3). Default zero is set.\n",(int)_F.length(),(int)_Mu.length());
1275 
1276  return false;
1277  }
1278 }
1279 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
1280 bool SensorLinkNewtonEuler::setSensor(const Matrix &_H, const Matrix &_COM, const double _m, const Matrix &_I)
1281 {
1282  if((_COM.rows()==4)&&(_COM.cols()==4) && (_H.cols()==4) && (_H.rows()==4) && (_I.rows()==3) && (_I.cols()==3))
1283  {
1284  H = _H; R = H.submatrix(0,2,0,2); r = H.subcol(0,3,3); r_proj = r*R;
1285  COM = _COM; RC = COM.submatrix(0,2,0,2); rc = COM.subcol(0,3,3); rc_proj = rc*R;
1286  I = _I;
1287  m = _m;
1288  return true;
1289  }
1290  else
1291  {
1292  m = _m;
1293  H.resize(4,4); H.eye();
1294  COM.resize(4,4); COM.eye();
1295  R = RC = eye(3,3);
1296  r = rc = r_proj = rc_proj = zeros(3);
1297  I.resize(3,3); I.zero();
1298  if(verbose)
1299  {
1300  fprintf(stderr,"SensorLink error, could not set properly H,COM,I due to wrong dimensions: \n");
1301  fprintf(stderr," H: (%zu,%zu) instead of (4,4) \n",_H.rows(),_H.cols());
1302  fprintf(stderr," COM: (%zu,%zu) instead of (4,4) \n",_COM.rows(),_COM.cols());
1303  fprintf(stderr," I: (%zu,%zu) instead of (3,3) \n",_I.rows(),_I.cols());
1304  fprintf(stderr,"Setting identities and zeros by default.");
1305  }
1306  return false;
1307  }
1308 }
1309 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
1310 string SensorLinkNewtonEuler::toString() const
1311 {
1312  string ret = "[Sensor link]: " + info + " [Mode]: " + NewEulMode_s[mode];
1313 
1314  char buffer[300]; int j=0;
1315  if(verbose)
1316  {
1317  j=sprintf( buffer," [F] : %.3f,%.3f,%.3f",F(0),F(1),F(2));
1318  j+=sprintf(buffer+j," [Mu] : %.3f,%.3f,%.3f",Mu(0),Mu(1),Mu(2));
1319  j+=sprintf(buffer+j," [R sens] : %.3f,%.3f,%.3f ; %.3f,%.3f,%.3f ; %.3f,%.3f,%.3f ",H(0,0),H(0,1),H(0,2),H(1,0),H(1,1),H(1,2),H(2,0),H(2,1),H(2,2));
1320  j+=sprintf(buffer+j," [p sens] : %.3f,%.3f,%.3f",H(0,3),H(1,3),H(2,3));
1321  }
1322  ret.append(buffer);
1323  return ret;
1324 }
1325 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
1326 void SensorLinkNewtonEuler::ForwardAttachToLink(iDynLink *link)
1327 {
1328  computeAngVel(link);
1329  computeAngAcc(link);
1330  computeLinAcc(link);
1331  computeLinAccC();
1332 }
1333 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
1334 void SensorLinkNewtonEuler::BackwardAttachToLink(iDynLink *link)
1335 {
1336  computeForce(link);
1337  computeMoment(link);
1338 }
1339 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
1340 void SensorLinkNewtonEuler::ForwardForcesMomentsToLink(iDynLink *link)
1341 {
1342  computeForceToLink(link);
1343  computeMomentToLink(link);
1344 }
1345 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
1346 
1347  //~~~~~~~~~~~~~~~~~~~~~~
1348  // get methods
1349  //~~~~~~~~~~~~~~~~~~~~~~
1350 
1351 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
1352 const Vector& SensorLinkNewtonEuler::getForce() const { return F;}
1353 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
1354 const Vector& SensorLinkNewtonEuler::getMoment(bool isBase) const { return Mu;}
1355 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
1356 const Vector& SensorLinkNewtonEuler::getAngVel() const { return w;}
1357 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
1358 const Vector& SensorLinkNewtonEuler::getAngAcc() const { return dw;}
1359 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
1360 const Vector& SensorLinkNewtonEuler::getLinAcc() const { return ddp;}
1361 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
1362 const Vector& SensorLinkNewtonEuler::getLinAccC() const { return ddpC;}
1363 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
1364 double SensorLinkNewtonEuler::getMass() const { return m;}
1365 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
1366 const Matrix& SensorLinkNewtonEuler::getInertia() const { return I;}
1367 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
1368 const Matrix& SensorLinkNewtonEuler::getR() { return R;}
1369 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
1370 const Matrix& SensorLinkNewtonEuler::getRC() { return RC;}
1371 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
1372 const Vector& SensorLinkNewtonEuler::getr(bool proj)
1373 {
1374  if(proj==false)
1375  return r;
1376  return r_proj;
1377 }
1378 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
1379 const Vector& SensorLinkNewtonEuler::getrC(bool proj)
1380 {
1381  if(proj==false)
1382  return rc;
1383  return rc_proj;
1384 }
1385 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
1386 double SensorLinkNewtonEuler::getIm() const {return 0.0;}
1387 double SensorLinkNewtonEuler::getFs() const {return 0.0;}
1388 double SensorLinkNewtonEuler::getFv() const {return 0.0;}
1389 double SensorLinkNewtonEuler::getD2q() const {return 0.0;}
1390 double SensorLinkNewtonEuler::getDq() const {return 0.0;}
1391 double SensorLinkNewtonEuler::getKr() const {return 0.0;}
1392 const Vector& SensorLinkNewtonEuler::getAngAccM() const {return zeros0;}
1393 double SensorLinkNewtonEuler::getTorque() const {return 0.0;}
1394 const Matrix& SensorLinkNewtonEuler::getH() const {return H;}
1395 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
1396 bool SensorLinkNewtonEuler::setForce (const Vector &_F)
1397 {
1398  if(_F.length()==3)
1399  {
1400  F=_F;
1401  return true;
1402  }
1403  else
1404  {
1405  if(verbose)
1406  fprintf(stderr,"SensorLink error, could not set force due to wrong dimension: %d instead of 3.\n",(int)_F.length());
1407 
1408  return false;
1409  }
1410 }
1411 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
1412 bool SensorLinkNewtonEuler::setMoment (const Vector &_Mu)
1413 {
1414  if(_Mu.length()==3)
1415  {
1416  Mu=_Mu;
1417  return true;
1418  }
1419  else
1420  {
1421  if(verbose)
1422  fprintf(stderr,"SensorLink error, could not set moment due to wrong dimension: %d instead of 3.\n",(int)_Mu.length());
1423 
1424  return false;
1425  }
1426 }
1427 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
1428 void SensorLinkNewtonEuler::setTorque (const double _Tau) {}
1429 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
1430 bool SensorLinkNewtonEuler::setAngVel (const Vector &_w)
1431 {
1432  if(_w.length()==3)
1433  {
1434  w = _w;
1435  return true;
1436  }
1437  else
1438  {
1439  if(verbose)
1440  fprintf(stderr,"SensorLink error, could not set w due to wrong size: %d instead of 3.\n",(int)_w.length());
1441 
1442  return false;
1443  }
1444 }
1445 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
1446 bool SensorLinkNewtonEuler::setAngAcc (const Vector &_dw)
1447 {
1448  if(_dw.length()==3)
1449  {
1450  dw = _dw;
1451  return true;
1452  }
1453  else
1454  {
1455  if(verbose)
1456  fprintf(stderr,"SensorLink error, could not set dw due to wrong size: %d instead of 3. \n",(int)_dw.length());
1457 
1458  return false;
1459  }
1460 }
1461 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
1462 bool SensorLinkNewtonEuler::setLinAcc (const Vector &_ddp)
1463 {
1464  if(_ddp.length()==3)
1465  {
1466  ddp = _ddp;
1467  return true;
1468  }
1469  else
1470  {
1471  if(verbose)
1472  fprintf(stderr,"SensorLink error, could not set ddp due to wrong size: %d instead of 3.\n",(int)_ddp.length());
1473 
1474  return false;
1475  }
1476 }
1477 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
1478 bool SensorLinkNewtonEuler::setLinAccC (const Vector &_ddpC)
1479 {
1480  if(_ddpC.length()==3)
1481  {
1482  ddpC = _ddpC;
1483  return true;
1484  }
1485  else
1486  {
1487  if(verbose)
1488  fprintf(stderr,"SensorLink error, could not set ddp due to wrong size: %d instead of 3.\n",(int)_ddpC.length());
1489 
1490  return false;
1491  }
1492 }
1493 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
1494 bool SensorLinkNewtonEuler::setAngAccM (const Vector &_dwM)
1495 {
1496  if(verbose)
1497  fprintf(stderr,"SensorLink error: no dwM existing \n");
1498  return false;
1499 }
1500 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
1501 
1502  //~~~~~~~~~~~~~~~~~~~~~~
1503  // redefined methods
1504  //~~~~~~~~~~~~~~~~~~~~~~
1505 
1506 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
1507 void SensorLinkNewtonEuler::computeAngVel( iDynLink *link)
1508 {
1509  w = link->getW() * R;
1510 }
1511 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
1512 void SensorLinkNewtonEuler::computeAngAcc( iDynLink *link)
1513 {
1514  dw = link->getdW() * R;
1515 }
1516 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
1517 void SensorLinkNewtonEuler::computeLinAcc( iDynLink *link)
1518 {
1519  switch(mode)
1520  {
1521  case DYNAMIC:
1522  case DYNAMIC_CORIOLIS_GRAVITY:
1523  case DYNAMIC_W_ROTOR:
1524  ddp = link->getLinAcc() * R;
1525  ddp += cross(dw, r_proj);
1526  ddp += cross(w, cross(w, r_proj));
1527  /*ddp = getR().transposed() * link->getLinAcc()
1528  - cross(dw,getr(true))
1529  - cross(w,cross(w,getr(true)));*/
1530  break;
1531  case STATIC:
1532  ddp = getR().transposed() * link->getLinAcc();
1533  break;
1534  }
1535 }
1536 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
1537 void SensorLinkNewtonEuler::computeLinAccC()
1538 {
1539  switch(mode)
1540  {
1541  case DYNAMIC:
1542  case DYNAMIC_CORIOLIS_GRAVITY:
1543  case DYNAMIC_W_ROTOR:
1544  ddpC = ddp;
1545  ddpC += cross(dw,rc);
1546  ddpC += cross(w,cross(w,rc));
1547  //ddpC = ddp + cross(dw,rc) + cross(w,cross(w,rc));
1548  break;
1549  case STATIC:
1550  ddpC = ddp;
1551  break;
1552  }
1553 }
1554 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
1555 void SensorLinkNewtonEuler::computeForce(iDynLink *link)
1556 {
1557  F = link->getForce() * R;
1558  F += m * ddpC;
1559  //F = getR().transposed() * link->getForce() + m * getLinAccC() ;
1560 }
1561 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
1562 void SensorLinkNewtonEuler::computeForceToLink ( iDynLink *link)
1563 {
1564  Vector temp = F;
1565  temp -= m*ddpC;
1566  link->setForce(R*temp);
1567  //link->setForce( R*( F - m * ddpC ) );
1568 }
1569 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
1570 void SensorLinkNewtonEuler::computeMomentToLink( iDynLink *link)
1571 {
1572  switch(mode)
1573  {
1574  case DYNAMIC_CORIOLIS_GRAVITY:
1575  case DYNAMIC:
1576  {
1577  Vector temp = Mu;
1578  temp += cross(r_proj, link->getForce()*R);
1579  temp -= cross(rc, m*ddpC);
1580  temp -= I * (dw);
1581  temp -= cross( w , I*(w));
1582  link->setMoment(R*temp);
1583  /*link->setMoment( getR()*( Mu + cross(getr(true),getR().transposed()*link->getForce())
1584  - cross(getrC(),(m * getLinAccC()))
1585  - getInertia() * getR().transposed() * getAngAcc()
1586  - cross( getR().transposed() * getAngVel() , getInertia() * getR().transposed() * getAngVel())
1587  ));*/
1588  break;
1589  }
1590  case DYNAMIC_W_ROTOR:
1591  link->setMoment( getR()*( Mu + cross(getr(true),getR().transposed()*link->getForce())
1592  - cross(getrC(),(m * getLinAccC()))
1593  - getInertia() * getAngAcc()
1594  - cross( getAngVel() , getInertia() * getAngVel())
1595  - link->getKr() * link->getD2Ang() * link->getIm() * getZM()
1596  - link->getKr() * link->getDAng() * link->getIm() * cross(getAngVel(),getZM())
1597  ));
1598  break;
1599  case STATIC:
1600  {
1601  Vector temp = Mu;
1602  temp += cross(r_proj, link->getForce()*R);
1603  temp -= cross(rc, m*ddpC);
1604  link->setMoment(R*temp);
1605  /*link->setMoment( getR() * ( Mu + cross(getr(true),getR().transposed()*link->getForce()) )
1606  - cross(getrC(), m*getLinAccC()) );*/
1607  break;
1608  }
1609  }
1610 }
1611 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
1612 void SensorLinkNewtonEuler::computeMoment(iDynLink *link)
1613 {
1614  switch(mode)
1615  {
1616  case DYNAMIC_CORIOLIS_GRAVITY:
1617  case DYNAMIC:
1618  Mu = cross(rc, m*ddpC);
1619  Mu -= cross(r_proj, link->getForce()*R);
1620  Mu += link->getMoment()*R;
1621  Mu += I * (dw);
1622  Mu += cross( w , I*(w));
1623  /*Mu = cross(getrC(),(m * getLinAccC()))
1624  - cross(getr(true),getR().transposed()*link->getForce())
1625  + getR().transposed() * link->getMoment()
1626  + getInertia() * getR().transposed() * getAngAcc()
1627  + cross( getR().transposed() * getAngVel() , getInertia() * getR().transposed() * getAngVel());*/
1628  break;
1629  case DYNAMIC_W_ROTOR:
1630  Mu = cross(getrC(),(m * getLinAccC()))
1631  - cross(getr(true),getR().transposed()*link->getForce())
1632  + getR().transposed() * link->getMoment()
1633  + getInertia() * getAngAcc()
1634  + cross( getAngVel() , getInertia() * getAngVel())
1635  + link->getKr() * link->getD2Ang() * link->getIm() * getZM()
1636  + link->getKr() * link->getDAng() * link->getIm() * cross(getAngVel(),getZM()) ;
1637 
1638  break;
1639  case STATIC:
1640  Mu = cross(rc, m*ddpC);
1641  Mu -= cross(r_proj, link->getForce()*R);
1642  Mu += link->getMoment()*R;
1643  /*Mu = cross(getrC(), m*getLinAccC())
1644  - cross(getr(true),getR().transposed()*link->getForce())
1645  + getR().transposed() * link->getMoment();*/
1646  break;
1647  }
1648 }
1649 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
1650 Vector SensorLinkNewtonEuler::getForceMoment() const
1651 {
1652  /*Vector ret(6); ret.zero();
1653  ret[0]=F[0]; ret[1]=F[1]; ret[2]=F[2];
1654  ret[3]=Mu[0]; ret[4]=Mu[1]; ret[5]=Mu[2];*/
1655  return cat(F, Mu);
1656 }
1657 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
1658 string SensorLinkNewtonEuler::getType() const
1659 {
1660  return "no type for the basic sensor - only iCub sensors have type";
1661 }
1662 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
1663 
1664 
1665 
1666 //================================
1667 //
1668 // ONE CHAIN NEWTON EULER
1669 //
1670 //================================
1671 
1672 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
1673 OneChainNewtonEuler::OneChainNewtonEuler(iDynChain *_c, string _info, const NewEulMode _mode, unsigned int verb)
1674 {
1675  //with pointers!
1676  chain = _c;
1677 
1678  string descript;
1679  char buffer[100]; int j=0;
1680 
1681  mode = _mode;
1682  verbose = verb;
1683  info = _info;
1684 
1685  //set the manipulator chain
1686  nLinks = _c->getN();
1687 
1688  //nLinks+base+final
1689  neChain = new OneLinkNewtonEuler*[nLinks+2];
1690 
1691  //first the base frame, for the forward - insertion as first
1692  neChain[0] = new BaseLinkNewtonEuler(chain->getH0(),mode,verbose);
1693 
1694  //then the link frames (the true ones)
1695  for(unsigned int i=0; i<nLinks; i++)
1696  {
1697  //OneLinkNewtonEuler * tmp
1698  neChain[i+1] = new OneLinkNewtonEuler(mode,verbose,chain->refLink(i));
1699  //insertion in the chain
1700  descript.clear();
1701  j=sprintf(buffer,"link <%d>",i);
1702  descript.append(buffer);
1703  neChain[i+1]->setInfo(descript);
1704  }
1705  //then the final frame, for the backward - insertion as last
1706  neChain[nLinks+1] = new FinalLinkNewtonEuler(chain->getHN(),mode,verbose);
1707 
1708  //the end effector is the last (nLinks+2-1 because it's an index)
1709  nEndEff = nLinks+1;
1710 
1711 }
1712 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
1713 OneChainNewtonEuler::~OneChainNewtonEuler()
1714 {
1715  for(unsigned int i=0;i<=nEndEff;i++)
1716  delete neChain[i];
1717  delete [] neChain;
1718 }
1719 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
1720 string OneChainNewtonEuler::toString() const
1721 {
1722  string ret;
1723  ret = "[Chain]: " + info;
1724  for(unsigned int i=0;i<=nEndEff;i++)
1725  {
1726  ret.append("\n");
1727  ret.append(neChain[i]->toString());
1728  }
1729  return ret;
1730 }
1731 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
1732 bool OneChainNewtonEuler::getVelAccAfterForward(unsigned int i, Vector &w, Vector &dw, Vector &dwM, Vector &ddp, Vector &ddpC) const
1733 {
1734  if(i<=nEndEff)
1735  {
1736  w = neChain[i]->getAngVel();
1737  dw = neChain[i]->getAngAcc();
1738  dwM = neChain[i]->getAngAccM();
1739  ddp = neChain[i]->getLinAcc();
1740  ddpC = neChain[i]->getLinAccC();
1741  return true;
1742  }
1743  else
1744  {
1745  if(verbose)
1746  fprintf(stderr,"OneChain error, impossible to retrieve vel/acc due to out of range index: %d > %d \n", i, nEndEff);
1747  return false;
1748  }
1749 }
1750 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
1751 bool OneChainNewtonEuler::getWrenchAfterForward(unsigned int i, Vector &F, Vector &Mu) const
1752 {
1753  if(i<=nEndEff)
1754  {
1755  F = neChain[i]->getForce();
1756  Mu = neChain[i]->getMoment(false);
1757  return true;
1758  }
1759  else
1760  {
1761  if(verbose)
1762  fprintf(stderr,"OneChain error, impossible to retrieve vel/acc due to out of range index: %d > %d \n",i,nEndEff);
1763  return false;
1764  }
1765 }
1766 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
1767 void OneChainNewtonEuler::getVelAccBase(Vector &w, Vector &dw,Vector &ddp) const
1768 {
1769  w = neChain[0]->getAngVel();
1770  dw = neChain[0]->getAngAcc();
1771  ddp = neChain[0]->getLinAcc();
1772 }
1773 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
1774 void OneChainNewtonEuler::getVelAccEnd(Vector &w, Vector &dw,Vector &ddp) const
1775 {
1776  w = neChain[nEndEff]->getAngVel();
1777  dw = neChain[nEndEff]->getAngAcc();
1778  ddp = neChain[nEndEff]->getLinAcc();
1779 }
1780 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
1781 void OneChainNewtonEuler::getWrenchBase(Vector &F, Vector &Mu) const
1782 {
1783  F = neChain[0]->getForce();
1784  Mu = neChain[0]->getMoment(false);
1785 }
1786 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
1787 void OneChainNewtonEuler::getWrenchEnd(Vector &F, Vector &Mu) const
1788 {
1789  F = neChain[nEndEff]->getForce();
1790  Mu = neChain[nEndEff]->getMoment(false);
1791 }
1792 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
1793 
1794  //~~~~~~~~~~~~~~~~~~~~~~
1795  // set methods
1796  //~~~~~~~~~~~~~~~~~~~~~~
1797 
1798 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
1799 void OneChainNewtonEuler::setVerbose(unsigned int verb)
1800 {
1801  verbose=verb;
1802  for(unsigned int i=0;i<=nEndEff;i++)
1803  neChain[i]->setVerbose(verb);
1804 }
1805 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
1806 void OneChainNewtonEuler::setMode(const NewEulMode _mode)
1807 {
1808  mode=_mode;
1809  for(unsigned int i=0;i<=nEndEff;i++)
1810  neChain[i]->setMode(_mode);
1811 }
1812 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
1813 void OneChainNewtonEuler::setInfo(const string _info)
1814 {
1815  info=_info;
1816 }
1817 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
1818 bool OneChainNewtonEuler::initKinematicBase(const Vector &w0,const Vector &dw0,const Vector &ddp0)
1819 {
1820  return neChain[0]->setAsBase(w0,dw0,ddp0);
1821 }
1822 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
1823 bool OneChainNewtonEuler::initKinematicEnd(const Vector &w0,const Vector &dw0,const Vector &ddp0)
1824 {
1825  return neChain[nEndEff]->setAsBase(w0,dw0,ddp0);
1826 }
1827 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
1828 bool OneChainNewtonEuler::initWrenchEnd(const Vector &Fend,const Vector &Muend)
1829 {
1830  return neChain[nEndEff]->setAsFinal(Fend,Muend);
1831 }
1832 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
1833 bool OneChainNewtonEuler::initWrenchBase(const Vector &Fend,const Vector &Muend)
1834 {
1835  return neChain[0]->setAsFinal(Fend,Muend);
1836 }
1837 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
1838 
1839  //~~~~~~~~~~~~~~~~~~~~~~
1840  // get methods
1841  //~~~~~~~~~~~~~~~~~~~~~~
1842 
1843 string OneChainNewtonEuler::getInfo() const {return info;}
1844 NewEulMode OneChainNewtonEuler::getMode() const {return mode;}
1845 
1846  //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
1847  // main computation methods
1848  //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
1849 
1850 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
1851 void OneChainNewtonEuler::ForwardKinematicFromBase()
1852 {
1853  for(unsigned int i=1;i<nEndEff;i++)
1854  {
1855  neChain[i]->ForwardKinematics(neChain[i-1]);
1856  }
1857 }
1858 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
1859 void OneChainNewtonEuler::ForwardKinematicFromBase(const Vector &_w, const Vector &_dw, const Vector &_ddp)
1860 {
1861  //set initial values on the base frame
1862  neChain[0]->setAsBase(_w,_dw,_ddp);
1863  //finally forward
1864  ForwardKinematicFromBase();
1865 }
1866 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
1867 void OneChainNewtonEuler::BackwardKinematicFromEnd()
1868 {
1869  for(unsigned int i=nEndEff;i>=1;i--)
1870  {
1871  neChain[i-1]->BackwardKinematics(neChain[i]);
1872  }
1873 }
1874 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
1875 void OneChainNewtonEuler::BackwardKinematicFromEnd(const Vector &_w, const Vector &_dw, const Vector &_ddp)
1876 {
1877  //set initial values on the base frame
1878  neChain[nEndEff]->setAsFinal(_w,_dw,_ddp);
1879  //finally forward
1880  BackwardKinematicFromEnd();
1881 }
1882 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
1883 void OneChainNewtonEuler::BackwardWrenchFromEnd()
1884 {
1885  for(int i=nEndEff-1; i>=0; i--)
1886  neChain[i]->BackwardWrench(neChain[i+1]);
1887 
1888  for(int i=nEndEff-1; i>0; i--)
1889  neChain[i]->computeTorque(neChain[i-1]);
1890 }
1891 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
1892 void OneChainNewtonEuler::computeTorques()
1893 {
1894  for(int i=nEndEff-1; i>0; i--){
1895  neChain[i]->computeTorque(neChain[i-1]);
1896  }
1897 }
1898 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
1899 void OneChainNewtonEuler::BackwardWrenchFromEnd(const Vector &F, const Vector &Mu)
1900 {
1901 
1902  //set initial values on the end-effector frame
1903  neChain[nEndEff]->setAsFinal(F,Mu);
1904  //then backward
1905  BackwardWrenchFromEnd();
1906 }
1907 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
1908 void OneChainNewtonEuler::ForwardWrenchFromBase()
1909 {
1910  fprintf(stderr,"ForwardWrenchFromBase: not implemented yet \n");
1911  /*for(int i=nEndEff-1; i>=0; i--)
1912  neChain[i]->BackwardWrench(neChain[i+1]);
1913  for(int i=nEndEff-1; i>0; i--)
1914  neChain[i]->computeTorque(neChain[i-1]); TO BE IMPLEMENTED*/
1915 }
1916 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
1917 void OneChainNewtonEuler::ForwardWrenchFromBase(const Vector &F, const Vector &Mu)
1918 {
1919  fprintf(stderr,"ForwardWrenchFromBase: not implemented yet \n");
1920  /*for(int i=nEndEff-1; i>=0; i--)
1921  neChain[i]->BackwardWrench(neChain[i+1]);
1922  for(int i=nEndEff-1; i>0; i--)
1923  neChain[i]->computeTorque(neChain[i-1]); TO BE IMPLEMENTED*/
1924 }
1925 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
1926 bool OneChainNewtonEuler::ForwardWrenchToEnd(unsigned int lSens)
1927 {
1928  if(lSens<nLinks)
1929  {
1930  // lSens is the sensor link index
1931  // link lSens --> neChain[lSens+1]
1932  // since ForwardWrench takes the previous, we start with the OneLink
1933  // indexed lSens+2 = lSens + baseLink + the next one
1934  // that's because link lSens = neChain[lSens+1] is already set before
1935  // with a specific sensor method
1936  for(unsigned int i=lSens+2; i<nEndEff; i++)
1937  neChain[i]->ForwardWrench(neChain[i-1]);
1938  return true;
1939  }
1940  else
1941  {
1942  fprintf(stderr,"OneChainNewtonEuler error, could not perform ForwardWrenchToEnd because of out of range index: %d >= %d \n",lSens,nLinks);
1943  return false;
1944  }
1945 }
1946 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
1947 bool OneChainNewtonEuler::BackwardWrenchToBase(unsigned int lSens)
1948 {
1949  if(lSens<nLinks)
1950  {
1951  // lSens is the sensor link index
1952  // link lSens --> neChain[lSens+1]
1953  // since BackwardWrench takes the next, we start with the OneLink
1954  // indexed lSens = lSens + baseLink - the previous one
1955  // that's because link lSens = neChain[lSens+1] is already set before
1956  // with a specific sensor method
1957  for(int i=lSens; i>=0; i--){
1958  neChain[i]->BackwardWrench(neChain[i+1]);
1959  }
1960  // now we can compute all torques
1961  // we also compute the one of the sensor link, since we needed the
1962  // previous link done
1963  for(int i=lSens+1; i>0; i--)
1964  neChain[i]->computeTorque(neChain[i-1]);
1965  return true;
1966  }
1967  else
1968  {
1969  fprintf(stderr,"OneChainNewtonEuler error, could not perform BackwardWrenchToBase because of out of range index: %d >= %d \n",lSens,nLinks);
1970  return false;
1971  }
1972 }
1973 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
1974 // new methods for contact
1975 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
1976 bool OneChainNewtonEuler::ForwardWrenchFromAtoB(unsigned int lA, unsigned int lB)
1977 {
1978  // lB must be larger than lA, because we are moving forward
1979  if( (lB >= lA) && (lB <=nLinks))
1980  {
1981  // link lA --> neChain[lA+1] (same for B)
1982  for(unsigned int i=lA+1; i<=lB+1; i++){
1983  // the torques are automatically computed inside the "ForwardWrench" method
1984  neChain[i]->ForwardWrench(neChain[i-1]);
1985  }
1986  return true;
1987  }
1988  else
1989  {
1990  fprintf(stderr,"OneChainNewtonEuler error, could not perform ForwardWrenchToEnd because of out of range index: A=%d must be < B=%d, and both < %d \n",lA,lB,nLinks);
1991  return false;
1992  }
1993 }
1994 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
1995 bool OneChainNewtonEuler::BackwardWrenchFromAtoB(unsigned int lA, unsigned int lB)
1996 {
1997  // lB must be smaller than lA, because we are moving backward
1998  if( (lB <= lA) && (lA<=nLinks) )
1999  {
2000  // link lA --> neChain[lA+1] (same for B)
2001  // note: it's int and not unsigned int to avoid problems when decrementing
2002  for(int i=lA+1; i>=(int)(lB+1); i--){
2003  neChain[i]->BackwardWrench(neChain[i+1]);
2004  }
2005 
2006  // Del Prete: now we can compute all torques
2007  for(int i=lA+2; i>=(int)(lB+2); i--)
2008  neChain[i]->computeTorque(neChain[i-1]);
2009  return true;
2010  }
2011  else
2012  {
2013  fprintf(stderr,"OneChainNewtonEuler error, could not perform BackwardWrenchFromAtoB because of out of range index: A=%d must be > B=%d, and both < %d \n",lA,lB,nLinks);
2014  return false;
2015  }
2016 }
2017 
2018 //======================================
2019 //
2020 // iDYN INV SENSOR
2021 //
2022 //======================================
2023 
2024 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
2025 iDynInvSensor::iDynInvSensor(iDynChain *_c, const string &_info, const NewEulMode _mode, unsigned int verb)
2026 {
2027  chain = _c;
2028  info = _info + "chain with sensor: unknown";
2029  mode = _mode;
2030  verbose = verb;
2031  //unknown sensor
2032  lSens = 0;
2033  sens = NULL;
2034 }
2035 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
2036 iDynInvSensor::iDynInvSensor(iDynChain *_c, unsigned int i, const Matrix &_H, const Matrix &_HC, const double _m, const Matrix &_I, const string &_info, const NewEulMode _mode, unsigned int verb)
2037 {
2038  chain = _c;
2039  info = _info + "chain with sensor: attached";
2040  mode = _mode;
2041  verbose = verb;
2042  // new sensor
2043  lSens = i;
2044  sens = new SensorLinkNewtonEuler(_H,_HC,_m,_I,_mode,verb);
2045 }
2046 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
2047 iDynInvSensor::~iDynInvSensor()
2048 {
2049  if(sens!=NULL)
2050  delete sens;
2051  sens=NULL;
2052 
2053  //do not delete the chain! only stop pointing at it!
2054  chain=NULL;
2055 }
2056 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
2057 string iDynInvSensor::toString() const
2058 {
2059  string ret;
2060  ret = "[Chain]: " + info + "\n[Sensor]: yes. ";
2061  char buffer[60];
2062  int j = sprintf(buffer,"[before link]: %d",lSens);
2063  ret.append(buffer);
2064  if(sens != NULL)
2065  ret = ret + sens->toString();
2066  return ret;
2067 }
2068 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
2069 bool iDynInvSensor::setSensor(unsigned int i, const Matrix &_H, const Matrix &_HC, const double _m, const Matrix &_I)
2070 {
2071  if(i<chain->getN())
2072  {
2073  lSens = i;
2074  if(sens==NULL)
2075  {
2076  sens = new SensorLinkNewtonEuler(_H,_HC,_m,_I,mode,verbose);
2077  return true;
2078  }
2079  else
2080  return sens->setSensor(_H,_HC,_m,_I);
2081  }
2082  else
2083  {
2084  if(verbose)
2085  fprintf(stderr,"iDynInvSensor error: could not set FT Sensor inside the dynamic chain due to out of range index: %d >= %d \n",i,chain->getN());
2086  return false;
2087  }
2088 }
2089 
2090 bool iDynInvSensor::setSensor(unsigned int i, SensorLinkNewtonEuler* sensor){
2091  if(i<chain->getN())
2092  {
2093  lSens = i;
2094  sens = sensor;
2095  return true;
2096  }
2097  if(verbose)
2098  fprintf(stderr,"iDynInvSensor error: could not set FT Sensor inside the dynamic chain due to out of range index: %d >= %d \n",i,chain->getN());
2099  return false;
2100 }
2101 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
2102 void iDynInvSensor::computeSensorForceMoment()
2103 {
2104  if(sens != NULL)
2105  {
2106  sens->ForwardAttachToLink(chain->refLink(lSens));
2107  sens->BackwardAttachToLink(chain->refLink(lSens));
2108 
2109  //if there's a contact in the link hosting the sensor
2110  // another function must be done!
2111  }
2112  else
2113  {
2114  fprintf(stderr,"iDynInvSensor error: could not make computations, the sensor is not set yet.\n");
2115  }
2116 }
2117 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
2118 Vector iDynInvSensor::getSensorForce() const
2119 {
2120  if(sens != NULL)
2121  return sens->getForce();
2122  else
2123  {
2124  if(verbose)
2125  fprintf(stderr,"iDynInvSensor error: could not get FT Sensor force, because the sensor is not set yet.\n");
2126  return Vector(0);
2127  }
2128 }
2129 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
2130 Vector iDynInvSensor::getSensorMoment()const
2131 {
2132  if(sens != NULL)
2133  return sens->getMoment(false);
2134  else
2135  {
2136  if(verbose)
2137  fprintf(stderr,"iDynInvSensor error: could not get FT Sensor moment, because the sensor is not set yet.\n");
2138  return Vector(0);
2139  }
2140 }
2141 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
2142 Vector iDynInvSensor::getTorques() const
2143 {
2144  return chain->getTorques();
2145 }
2146 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
2147 
2148  //~~~~~~~~~~~~~~
2149  // set methods
2150  //~~~~~~~~~~~~~~
2151 
2152 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
2153 void iDynInvSensor::setMode(const NewEulMode _mode)
2154 {
2155  mode = _mode;
2156  if(sens != NULL) sens->setMode(_mode);
2157 }
2158 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
2159 void iDynInvSensor::setVerbose(unsigned int verb)
2160 {
2161  verbose = verb;
2162  if(sens != NULL) sens->setVerbose(verb);
2163 }
2164 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
2165 void iDynInvSensor::setInfo(const string &_info)
2166 {
2167  info = _info;
2168 }
2169 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
2170 void iDynInvSensor::setSensorInfo(const string &_info)
2171 {
2172  if(sens != NULL) sens->setInfo(_info);
2173 }
2174 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
2175 bool iDynInvSensor::setDynamicParameters(const double _m, const yarp::sig::Matrix &_HC, const yarp::sig::Matrix &_I)
2176 {
2177  if( sens != NULL )
2178  {
2179  Matrix H_pass = sens->getH();
2180  return sens->setSensor(H_pass,_HC,_m,_I);
2181  }
2182  else
2183  {
2184  if(verbose) fprintf(stderr,"iDynInvSensor error: could not set dynamic parameters, because the sensor is not set yet.\n");
2185  return false;
2186  }
2187 }
2188 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
2189 
2190  //~~~~~~~~~~~~~~
2191  // get methods
2192  //~~~~~~~~~~~~~~
2193 
2194 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
2195 string iDynInvSensor::getInfo() const {return info;}
2196 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
2197 Matrix iDynInvSensor::getH() const
2198 {
2199  if(sens != NULL)
2200  return sens->getH();
2201  else
2202  {
2203  if(verbose)
2204  fprintf(stderr,"iDynInvSensor error: could not get H of the FT sensor, because the sensor is not set yet.\n");
2205  return Matrix(0,0);
2206  }
2207 
2208 }
2209 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
2210 double iDynInvSensor::getMass() const
2211 {
2212  if(sens != NULL)
2213  return sens->getMass();
2214  else
2215  {
2216  if(verbose)
2217  fprintf(stderr,"iDynInvSensor error: could not get mass of the FT sensor, because the sensor is not set yet.\n");
2218  return 0;
2219  }
2220 
2221 }
2222 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
2223 Matrix iDynInvSensor::getCOM() const
2224 {
2225  Matrix com = Matrix(4,4);
2226  if(sens != NULL) {
2227  com.setSubmatrix(sens->getRC(),0,0);
2228  com.setSubcol(sens->getrC(),0,3);
2229  return com;
2230  }
2231  else
2232  {
2233  if(verbose)
2234  fprintf(stderr,"iDynInvSensor error: could not get COM of the FT sensor, because the sensor is not set yet.\n");
2235  return Matrix(0,0);
2236  }
2237 
2238 }
2239 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
2240 Matrix iDynInvSensor::getInertia() const
2241 {
2242  if(sens != NULL)
2243  return sens->getInertia();
2244  else
2245  {
2246  if(verbose)
2247  fprintf(stderr,"iDynInvSensor error: could not get inertia of the FT sensor, because the sensor is not set yet.\n");
2248  return Matrix(0,0);
2249  }
2250 
2251 }
2252 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
2253 string iDynInvSensor::getSensorInfo() const
2254 {
2255  if(sens != NULL)
2256  return sens->getInfo();
2257  else
2258  {
2259  if(verbose)
2260  fprintf(stderr,"iDynInvSensor error: could not get info from the FT sensor, because the sensor is not set yet.\n");
2261  return "FT sensor is not set";
2262  }
2263 }
2264 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
2265 Vector iDynInvSensor::getSensorForceMoment() const
2266 {
2267  if(sens != NULL)
2268  return sens->getForceMoment();
2269  else
2270  {
2271  if(verbose)
2272  fprintf(stderr,"iDynInvSensor error: could not get force-moment of the FT sensor, because the sensor is not set yet.\n");
2273  return Vector(0);
2274  }
2275 }
2276 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
2277 unsigned int iDynInvSensor::getSensorLink() const {return lSens; }
2278 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
2279 
2280 
2281 //======================================
2282 //
2283 // iCUB ARM SENSOR LINK
2284 //
2285 //======================================
2286 
2287 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
2288 iCubArmSensorLink::iCubArmSensorLink(const string &_type, const NewEulMode _mode, unsigned int verb)
2289 :SensorLinkNewtonEuler(_mode,verb)
2290 {
2291  // the arm type determines the sensor properties
2292  type = _type;
2293  //now setting inertia, mass and COM specifically for each link
2294  H.resize(4,4); COM.resize(4,4); I.resize(3,3);
2295  if(type=="left")
2296  {
2297  H.zero(); H(0,0) = 1.0; H(2,1) = 1.0; H(1,2) = -1.0; H(1,3) = 0.08428; H(3,3) = 1.0;
2298  COM.eye(); COM(0,3) = -1.56e-04; COM(1,3) = -9.87e-05; COM(2,3) = 2.98e-2;
2299  I.zero(); I(0,0) = 4.08e-04; I(0,1) = I(1,0) = -1.08e-6; I(0,2) = I(2,0) = -2.29e-6;
2300  I(1,1) = 3.80e-04; I(1,2) = I(2,1) = 3.57e-6; I(2,2) = 2.60e-4;
2301  m = 7.2784301e-01;
2302  }
2303  else
2304  {
2305  if(!(type =="right"))
2306  {
2307  if(verbose)
2308  fprintf(stderr,"iCubArmSensorLink error: type is not left/right: assuming right.\n");
2309  type = "right";
2310  }
2311 
2312  H.zero(); H(0,0) = -1.0; H(2,1) = 1.0; H(1,2) = 1.0; H(1,3) = -0.08428; H(3,3) = 1.0;
2313  COM.eye(); COM(0,3) = -1.5906019e-04; COM(1,3) = 8.2873258e-05; COM(2,3) = 2.9882773e-02;
2314  I.zero(); I(0,0) = 4.08e-04; I(0,1) = I(1,0) = -1.08e-6; I(0,2) = I(2,0) = -2.29e-6;
2315  I(1,1) = 3.80e-04; I(1,2) = I(2,1) = 3.57e-6; I(2,2) = 2.60e-4;
2316  m = 7.29e-01;
2317  }
2318  R = H.submatrix(0,2,0,2); r = H.subcol(0,3,3); r_proj = r*R;
2319  RC = COM.submatrix(0,2,0,2); rc = COM.subcol(0,3,3); rc_proj = rc*R;
2320 
2321  //then the sensor information
2322  info.clear(); info = "FT sensor " + type + " arm";
2323 }
2324 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
2325 string iCubArmSensorLink::getType() const
2326 {
2327  return type;
2328 }
2329 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
2330 
2331 //======================================
2332 //
2333 // iDYN INV SENSOR ARM
2334 //
2335 //======================================
2336 
2337 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
2338 iDynInvSensorArm::iDynInvSensorArm(iCubArmDyn *_c, const NewEulMode _mode, unsigned int verb)
2339 :iDynInvSensor(_c->asChain(),_c->getType(),_mode,verb)
2340 {
2341  // FT sensor is in position 5 in the kinematic chain in both arms
2342  lSens = 5;
2343  // the arm type determines the sensor properties
2344  if( !((_c->getType()=="left")||(_c->getType()=="right")) )
2345  {
2346  if(verbose)
2347  {
2348  fprintf(stderr,"iDynInvSensorArm error: type is not left/right. iCub only has a left and a right arm, it is not an octopus :) \n");
2349  fprintf(stderr,"iDynInvSensorArm: assuming right arm. \n");
2350  }
2351  // set the sensor with the default value
2352  sens = new iCubArmSensorLink("right",mode,verbose);
2353  }
2354  else
2355  {
2356  // set the sensor properly
2357  sens = new iCubArmSensorLink(_c->getType(),mode,verbose);
2358  }
2359 }
2360 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
2361 iDynInvSensorArm::iDynInvSensorArm(iDynChain *_c, const string _type, const NewEulMode _mode, unsigned int verb)
2362 :iDynInvSensor(_c,_type,_mode,verb)
2363 {
2364  // FT sensor is in position 5 in the kinematic chain in both arms
2365  lSens = 5;
2366  // the arm type determines the sensor properties
2367  if( !((_type=="left")||(_type=="right")) )
2368  {
2369  if(verbose)
2370  {
2371  fprintf(stderr,"iDynInvSensorArm error: type is not left/right. iCub only has a left and a right arm, it is not an octopus :) \n");
2372  fprintf(stderr,"iDynInvSensorArm: assuming right arm.\n");
2373  }
2374  // set the sensor with the default value
2375  sens = new iCubArmSensorLink("right",mode,verbose);
2376  }
2377  else
2378  {
2379  // set the sensor properly
2380  sens = new iCubArmSensorLink(_type,mode,verbose);
2381  }
2382 }
2383 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
2384 string iDynInvSensorArm::getType() const
2385 {
2386  return sens->getType();
2387 }
2388 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
2389 
2390 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
2391 
2392 //=========================================
2393 //
2394 // iDYN INV SENSOR ARM NO TORSO
2395 //
2396 //=========================================
2397 
2398 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
2399 iDynInvSensorArmNoTorso::iDynInvSensorArmNoTorso(iCubArmNoTorsoDyn *_c, const NewEulMode _mode, unsigned int verb)
2400 :iDynInvSensor(_c->asChain(),_c->getType(),_mode,verb)
2401 {
2402  // FT sensor is in position 2 in the kinematic chain in both arms
2403  // note: it's 5 if arm with torso, 2 if arm without torso
2404  lSens = 2;
2405  // the arm type determines the sensor properties
2406  if( !((_c->getType()=="left")||(_c->getType()=="right")) )
2407  {
2408  if(verbose)
2409  {
2410  fprintf(stderr,"iDynInvSensorArm error: type is not left/right. iCub only has a left and a right arm, it is not an octopus :) \n");
2411  fprintf(stderr,"iDynInvSensorArm: assuming right arm. \n");
2412  }
2413  // set the sensor with the default value
2414  sens = new iCubArmSensorLink("right",mode,verbose);
2415  }
2416  else
2417  {
2418  // set the sensor properly
2419  sens = new iCubArmSensorLink(_c->getType(),mode,verbose);
2420  }
2421 }
2422 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
2423 iDynInvSensorArmNoTorso::iDynInvSensorArmNoTorso(iDynChain *_c, const string _type, const NewEulMode _mode, unsigned int verb)
2424 :iDynInvSensor(_c,_type,_mode,verb)
2425 {
2426  // FT sensor is in position 2 in the kinematic chain in both arms
2427  // note: it's 5 if arm with torso, 2 if arm without torso
2428  lSens = 2;
2429  // the arm type determines the sensor properties
2430  if( !((_type=="left")||(_type=="right")) )
2431  {
2432  if(verbose)
2433  {
2434  fprintf(stderr,"iDynInvSensorArm error: type is not left/right. iCub only has a left and a right arm, it is not an octopus :) \n");
2435  fprintf(stderr,"iDynInvSensorArm: assuming right arm. \n");
2436  }
2437  // set the sensor with the default value
2438  sens = new iCubArmSensorLink("right",mode,verbose);
2439  }
2440  else
2441  {
2442  // set the sensor properly
2443  sens = new iCubArmSensorLink(_type,mode,verbose);
2444  }
2445 }
2446 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
2447 string iDynInvSensorArmNoTorso::getType() const
2448 {
2449  return sens->getType();
2450 }
2451 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
2452 
2453 //======================================
2454 //
2455 // iCUB LEG SENSOR LINK
2456 //
2457 //======================================
2458 
2459 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
2460 iCubLegSensorLink::iCubLegSensorLink(const string _type, const NewEulMode _mode, unsigned int verb)
2461 :SensorLinkNewtonEuler(_mode,verb)
2462 {
2463  // the arm type determines the sensor properties
2464  type = _type;
2465  //now setting inertia, mass and COM specifically for the link
2466  H.resize(4,4); COM.resize(4,4); I.resize(3,3);
2467  if(type=="left")
2468  {
2469  H.zero(); H(0,1) = -1.0; H(1,0) = -1.0; H(2,2) = -1.0; H(2,3) = -0.0665; H(3,3) = 1.0;
2470  COM.zero();
2471  I.zero();
2472 
2473  m = 0.0;
2474  }
2475  else
2476  {
2477  if(!(type =="right"))
2478  {
2479  if(verbose)
2480  fprintf(stderr,"iCubLegSensorLink error: type is not left/right: assuming right. \n");
2481  type = "right";
2482  }
2483 
2484 
2485  H.zero(); H(0,1) = 1.0; H(1,0) = -1.0; H(2,2) = 1.0; H(2,3) = 0.0665; H(3,3) = 1.0;
2486  COM.zero();
2487  I.zero();
2488 
2489  m = 0.0;
2490  }
2491  R = H.submatrix(0,2,0,2); r = H.subcol(0,3,3); r_proj = r*R;
2492  RC = COM.submatrix(0,2,0,2); rc = COM.subcol(0,3,3); rc_proj = rc*R;
2493 
2494  //then the sensor information
2495  info.clear(); info = "FT sensor " + type + " leg";
2496 }
2497 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
2498 string iCubLegSensorLink::getType() const
2499 {
2500  return type;
2501 }
2502 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
2503 
2504 
2505 //======================================
2506 //
2507 // iDYN INV SENSOR LEG
2508 //
2509 //======================================
2510 
2511 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
2512 iDynInvSensorLeg::iDynInvSensorLeg(iCubLegDyn *_c, const NewEulMode _mode, unsigned int verb)
2513 :iDynInvSensor(_c->asChain(),_c->getType(),_mode,verb)
2514 {
2515  // FT sensor is in position 1 in the kinematic chain in both legs
2516  lSens = 1;
2517  // the leg type determines the sensor properties
2518  if( !((_c->getType()=="left")||(_c->getType()=="right")) )
2519  {
2520  if(verbose)
2521  {
2522  fprintf(stderr,"iDynInvSensorLeg error: type is not left/right. iCub only has a left and a right leg, it is not a millipede :) \n");
2523  fprintf(stderr,"iDynInvSensorLeg: assuming right leg.\n");
2524  }
2525  // set the sensor with the default value
2526  sens = new iCubLegSensorLink("right",mode,verbose);
2527  }
2528  else
2529  {
2530  // set the sensor properly
2531  sens = new iCubLegSensorLink(_c->getType(),mode,verbose);
2532  }
2533 }
2534 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
2535 iDynInvSensorLeg::iDynInvSensorLeg(iDynChain *_c, const string _type, const NewEulMode _mode, unsigned int verb)
2536 :iDynInvSensor(_c,_type,_mode,verb)
2537 {
2538  // FT sensor is in position 2 in the kinematic chain in both legs
2539  lSens = 1;
2540  // the leg type determines the sensor properties
2541  if( !((_type=="left")||(_type=="right")) )
2542  {
2543  if(verbose)
2544  {
2545  fprintf(stderr,"iDynInvSensorLeg error: type is not left/right. iCub only has a left and a right leg, it is not a millipede :) \n");
2546  fprintf(stderr,"iDynInvSensorLeg: assuming right leg.\n");
2547  }
2548  // set the sensor with the default value
2549  sens = new iCubLegSensorLink("right",mode,verbose);
2550  }
2551  else
2552  {
2553  // set the sensor properly
2554  sens = new iCubLegSensorLink(_type,mode,verbose);
2555  }
2556 }
2557 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
2558 string iDynInvSensorLeg::getType() const
2559 {
2560  return sens->getType();
2561 }
2562 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
2563 
2564 
2565 
2566 
2567 //================================
2568 //
2569 // I DYN SENSOR
2570 //
2571 //================================
2572 
2573 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
2574 iDynSensor::iDynSensor(iDynChain *_c, string _info, const NewEulMode _mode, unsigned int verb)
2575 :iDynInvSensor(_c, _info, _mode, verb)
2576 {}
2577 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
2578 iDynSensor::iDynSensor(iDynChain *_c, unsigned int i, const Matrix &_H, const Matrix &_HC, const double _m, const Matrix &_I, string _info, const NewEulMode _mode, unsigned int verb)
2579 :iDynInvSensor(_c, i, _H, _HC, _m, _I, _info, _mode, verb)
2580 {}
2581 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
2582 bool iDynSensor::setSensorMeasures(const Vector &F, const Vector &Mu)
2583 {
2584  return sens->setMeasuredFMu(F,Mu);
2585 }
2586 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
2587 bool iDynSensor::setSensorMeasures(const Vector &FM)
2588 {
2589  if(FM.length()==6)
2590  {
2591  Vector f(3); f[0]=FM[0];f[1]=FM[1];f[2]=FM[2];
2592  Vector m(3); m[0]=FM[3];m[1]=FM[4];m[2]=FM[5];
2593  return sens->setMeasuredFMu(f,m);
2594  }
2595  else
2596  {
2597  if(verbose)
2598  {
2599  fprintf(stderr,"iDynSensor error: could not set sensor measures due to wrong sized vector: %d instead of 6 (3+3).\n",(int)FM.length());
2600  }
2601  return false;
2602  }
2603 }
2604 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
2605 
2606  //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
2607  // main computation methods
2608  //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
2609 
2610 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
2611 void iDynSensor::computeFromSensorNewtonEuler()
2612 {
2613  //first forward all the quantities w,dw,.. in the chain
2614  //just in case it was not initialized before
2615  if(chain->NE == NULL)
2616  {
2617  chain->prepareNewtonEuler(mode);
2618  chain->initNewtonEuler();
2619  }
2620  //the iDynChain independently solve the forward phase of the limb
2621  //setting w,dw,ddp,ddpC
2622  chain->computeKinematicNewtonEuler();
2623  sens->ForwardAttachToLink(chain->refLink(lSens));
2624  //the sensor does not need to retrieve w,dw,ddp,ddpC in this case
2625  //then propagate forces and moments
2626  //from sensor to lSens
2627  sens->ForwardForcesMomentsToLink(chain->refLink(lSens));
2628  //from lSens to End
2629  chain->NE->ForwardWrenchToEnd(lSens);
2630  //from lSens to Base
2631  chain->NE->BackwardWrenchToBase(lSens);
2632 }
2633 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
2634 void iDynSensor::computeWrenchFromSensorNewtonEuler()
2635 {
2636  // this must be done
2637  sens->ForwardAttachToLink(chain->refLink(lSens));
2638  //propagate forces and moments
2639  //from sensor to lSens
2640  sens->ForwardForcesMomentsToLink(chain->refLink(lSens));
2641  //from lSens to End
2642  chain->NE->ForwardWrenchToEnd(lSens);
2643  //from lSens to Base
2644  chain->NE->BackwardWrenchToBase(lSens);
2645 }
2646 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
2647 bool iDynSensor::computeFromSensorNewtonEuler(const Vector &F, const Vector &Mu)
2648 {
2649  //set measured F/Mu on the sensor
2650  if(setSensorMeasures(F,Mu))
2651  {
2652  computeFromSensorNewtonEuler();
2653  return true;
2654  }
2655  else
2656  return false;
2657 }
2658 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
2659 bool iDynSensor::computeFromSensorNewtonEuler(const Vector &FMu)
2660 {
2661  //set measured F/Mu on the sensor
2662  if(setSensorMeasures(FMu))
2663  {
2664  computeFromSensorNewtonEuler();
2665  return true;
2666  }
2667  else
2668  return false;
2669 }
2670 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
2671 
2672 
2673  //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
2674  // get methods, overloaded from iDyn
2675  //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
2676 
2677 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
2678 Matrix iDynSensor::getForces() const {return chain->getForces();}
2679 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
2680 Matrix iDynSensor::getMoments() const {return chain->getMoments();}
2681 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
2682 Vector iDynSensor::getTorques() const {return chain->getTorques();}
2683 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
2684 Vector iDynSensor::getForce(const unsigned int iLink) const {return chain->getForce(iLink);}
2685 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
2686 Vector iDynSensor::getMoment(const unsigned int iLink) const {return chain->getMoment(iLink);}
2687 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
2688 double iDynSensor::getTorque(const unsigned int iLink) const {return chain->getTorque(iLink);}
2689 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
2690 Matrix iDynSensor::getForcesNewtonEuler() const {return chain->getForcesNewtonEuler();}
2691 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
2692 Matrix iDynSensor::getMomentsNewtonEuler() const {return chain->getMomentsNewtonEuler();}
2693 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
2694 Vector iDynSensor::getTorquesNewtonEuler() const {return chain->getTorquesNewtonEuler();}
2695 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
2696 Vector iDynSensor::getForceMomentEndEff() const {return chain->getForceMomentEndEff();}
2697 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
2698 
2699 
2700 //======================================
2701 //
2702 // iDYN SENSOR ARM
2703 //
2704 //======================================
2705 
2706 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
2707 iDynSensorArm::iDynSensorArm(iCubArmDyn *_c, const NewEulMode _mode, unsigned int verb)
2708 :iDynSensor(_c->asChain(),_c->getType(),_mode,verb)
2709 {
2710  // FT sensor is in position 5 in the kinematic chain in both arms
2711  lSens = 5;
2712  // the arm type determines the sensor properties
2713  if( !((_c->getType()=="left")||(_c->getType()=="right")) )
2714  {
2715  if(verbose)
2716  {
2717  fprintf(stderr,"iDynSensorArm error: type is not left/right. iCub only has a left and a right arm, it is not an octopus :) \n");
2718  fprintf(stderr,"iDynSensorArm: assuming right arm.\n");
2719  }
2720  // set the sensor with the default value
2721  sens = new iCubArmSensorLink("right",mode,verbose);
2722  }
2723  else
2724  {
2725  // set the sensor properly
2726  sens = new iCubArmSensorLink(_c->getType(),mode,verbose);
2727  }
2728 }
2729 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
2730 string iDynSensorArm::getType() const
2731 {
2732  return sens->getType();
2733 }
2734 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
2735 
2736 
2737 
2738 
2739 //======================================
2740 //
2741 // iDYN SENSOR ARM NO TORSO
2742 //
2743 //======================================
2744 
2745 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
2746 iDynSensorArmNoTorso::iDynSensorArmNoTorso(iCubArmNoTorsoDyn *_c, const NewEulMode _mode, unsigned int verb)
2747 :iDynSensor(_c->asChain(),_c->getType(),_mode,verb)
2748 {
2749  // FT sensor is in position 2 in the kinematic chain in both arms
2750  // note position 5 if with torso, 2 without torso
2751  lSens = 2;
2752  // the arm type determines the sensor properties
2753  if( !((_c->getType()=="left")||(_c->getType()=="right")) )
2754  {
2755  if(verbose)
2756  {
2757  fprintf(stderr,"iDynSensorArm error: type is not left/right. iCub only has a left and a right arm, it is not an octopus :) \n");
2758  fprintf(stderr,"iDynSensorArm: assuming right arm.\n");
2759  }
2760  // set the sensor with the default value
2761  sens = new iCubArmSensorLink("right",mode,verbose);
2762  }
2763  else
2764  {
2765  // set the sensor properly
2766  sens = new iCubArmSensorLink(_c->getType(),mode,verbose);
2767  }
2768 }
2769 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
2770 string iDynSensorArmNoTorso::getType() const
2771 {
2772  return sens->getType();
2773 }
2774 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
2775 
2776 
2777 
2778 
2779 
2780 //======================================
2781 //
2782 // iDYN INV SENSOR LEG
2783 //
2784 //======================================
2785 
2786 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
2787 iDynSensorLeg::iDynSensorLeg(iCubLegDyn *_c, const NewEulMode _mode, unsigned int verb)
2788 :iDynSensor(_c->asChain(),_c->getType(),_mode,verb)
2789 {
2790  // FT sensor is in position 2 in the kinematic chain in both legs
2791  lSens = 1;
2792  // the leg type determines the sensor properties
2793  if( !((_c->getType()=="left")||(_c->getType()=="right")) )
2794  {
2795  if(verbose)
2796  {
2797  fprintf(stderr,"iDynSensorLeg error: type is not left/right. iCub only has a left and a right leg, it is not a millipede :) \n");
2798  fprintf(stderr,"iDynSensorLeg: assuming right leg.\n");
2799  }
2800  // set the sensor with the default value
2801  sens = new iCubLegSensorLink("right",mode,verbose);
2802  }
2803  else
2804  {
2805  // set the sensor properly
2806  sens = new iCubLegSensorLink(_c->getType(),mode,verbose);
2807  }
2808 }
2809 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
2810 string iDynSensorLeg::getType() const
2811 {
2812  return sens->getType();
2813 }
2814 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
2815 
2816 
iCub::iDyn::SensorLinkNewtonEuler::RC
yarp::sig::Matrix RC
Definition: iDynInv.h:744
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::initNewtonEuler
bool initNewtonEuler(const yarp::sig::Vector &w0, const yarp::sig::Vector &dw0, const yarp::sig::Vector &ddp0, const yarp::sig::Vector &Fend, const yarp::sig::Vector &Muend)
Initialize the Newton-Euler method by setting the base (virtual link) velocity and accelerations ( w0...
iCub::iDyn::SensorLinkNewtonEuler::getDq
double getDq() const
Definition: iDynInv.cpp:1390
iCub::iDyn::iDynInvSensor
Definition: iDynInv.h:1222
iCub::iDyn::BaseLinkNewtonEuler::getMoment
const yarp::sig::Vector & getMoment(bool isBase=false) const
Definition: iDynInv.cpp:884
iCub::iDyn::OneLinkNewtonEuler::verbose
unsigned int verbose
verbosity flag
Definition: iDynInv.h:104
iCub::iDyn::SensorLinkNewtonEuler::F
yarp::sig::Vector F
measured or estimated force
Definition: iDynInv.h:722
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::BaseLinkNewtonEuler::w
yarp::sig::Vector w
initial angular velocity
Definition: iDynInv.h:495
iKinFwd.h
iCub::iDyn::iDynInvSensor::getInertia
yarp::sig::Matrix getInertia() const
Get the inertia of the portion of link defined between sensor and i-th frame.
Definition: iDynInv.cpp:2240
iCub::iDyn::OneLinkNewtonEuler::getAngVel
virtual const yarp::sig::Vector & getAngVel() const
Definition: iDynInv.cpp:338
iCub::iDyn::SensorLinkNewtonEuler::setLinAccC
bool setLinAccC(const yarp::sig::Vector &_ddpC)
Set the OneLink linear acceleration of the COM (ddpC), ie the corresponding iDynLink linear accelerat...
Definition: iDynInv.cpp:1478
iCub::skinDynLib
Definition: common.h:35
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::FinalLinkNewtonEuler::getAngVel
const yarp::sig::Vector & getAngVel() const
Definition: iDynInv.cpp:1127
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::FinalLinkNewtonEuler::getDq
double getDq() const
Definition: iDynInv.cpp:1140
iCub::iDyn::FinalLinkNewtonEuler::getMoment
const yarp::sig::Vector & getMoment(bool isBase=false) const
Definition: iDynInv.cpp:1125
iCub::iDyn::FinalLinkNewtonEuler::getForce
const yarp::sig::Vector & getForce() const
Definition: iDynInv.cpp:1123
iCub::iDyn::SensorLinkNewtonEuler::getFs
double getFs() const
Definition: iDynInv.cpp:1387
iCub::iDyn::SensorLinkNewtonEuler::getFv
double getFv() const
Definition: iDynInv.cpp:1388
iCub::iDyn::FinalLinkNewtonEuler::getLinAccC
const yarp::sig::Vector & getLinAccC() const
Definition: iDynInv.cpp:1131
iCub::iDyn::iDynSensor::iDynSensor
iDynSensor(iDyn::iDynChain *_c, std::string _info, const NewEulMode _mode=DYNAMIC, unsigned int verb=iCub::skinDynLib::NO_VERBOSE)
Constructor without FT sensor: the sensor must be set with setSensor()
Definition: iDynInv.cpp:2574
iCub::action::log::info
@ info
Definition: actionPrimitives.cpp:64
iCub::iDyn::SensorLinkNewtonEuler::getForceMoment
yarp::sig::Vector getForceMoment() const
Get the sensor force and moment in a single (6x1) vector.
Definition: iDynInv.cpp:1650
iCub::iDyn::iDynInvSensorArm::iDynInvSensorArm
iDynInvSensorArm(iDyn::iCubArmDyn *_c, const NewEulMode _mode=DYNAMIC, unsigned int verb=iCub::skinDynLib::NO_VERBOSE)
Constructor: the sensor is automatically set with "right" or "left" choice.
Definition: iDynInv.cpp:2338
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::OneChainNewtonEuler::verbose
unsigned int verbose
verbosity flag
Definition: iDynInv.h:902
iCub::iDyn::FinalLinkNewtonEuler::w
yarp::sig::Vector w
initial angular velocity
Definition: iDynInv.h:614
iCub::iDyn::iDynChain
Definition: iDyn.h:532
iCub::iDyn::OneChainNewtonEuler::BackwardWrenchFromAtoB
bool BackwardWrenchFromAtoB(unsigned int lA, unsigned int lB)
Base function for inverse Newton-Euler: from the lA-th link to the lB-th, backward of forces and mome...
Definition: iDynInv.cpp:1995
iCub::iDyn::iDynSensor::getForce
yarp::sig::Vector getForce(const unsigned int iLink) const
Returns the i-th link force.
Definition: iDynInv.cpp:2684
iCub::iDyn::SensorLinkNewtonEuler::rc
yarp::sig::Vector rc
Definition: iDynInv.h:747
iCub::iDyn::BaseLinkNewtonEuler::setMoment
bool setMoment(const yarp::sig::Vector &_Mu)
Set the OneLink moment: either the corresponding iDynLink moment, or the one declared as member in th...
Definition: iDynInv.cpp:920
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::info
std::string info
info or useful notes
Definition: iDynInv.h:900
iCub::iDyn::SensorLinkNewtonEuler::computeForce
void computeForce(iDynLink *link)
Definition: iDynInv.cpp:1555
iCub::iDyn::FinalLinkNewtonEuler::getRC
const yarp::sig::Matrix & getRC()
Definition: iDynInv.cpp:1135
iCub::iDyn::iDynChain::computeKinematicNewtonEuler
void computeKinematicNewtonEuler()
Calls the proper method to compute kinematics variables: either ForwardKinematicFromBase() or Backwar...
Definition: iDyn.cpp:992
iCub::iDyn::iDynInvSensor::getSensorForce
yarp::sig::Vector getSensorForce() const
Returns the sensor estimated force.
Definition: iDynInv.cpp:2118
iDynInv.h
iCub::iDyn::BaseLinkNewtonEuler::setAngAcc
bool setAngAcc(const yarp::sig::Vector &_dw)
Set the OneLink angular acceleration (dw), ie the corresponding iDynLink angular acceleration (dw) (i...
Definition: iDynInv.cpp:955
iCub::iDyn::iDynInvSensor::getMass
double getMass() const
Get the mass of the portion of link defined between sensor and i-th frame.
Definition: iDynInv.cpp:2210
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::OneLinkNewtonEuler::getAngAcc
virtual const yarp::sig::Vector & getAngAcc() const
Definition: iDynInv.cpp:340
iCub::iDyn::SensorLinkNewtonEuler::getTorque
double getTorque() const
Definition: iDynInv.cpp:1393
iCub::iDyn::iDynInvSensorArm::getType
std::string getType() const
Definition: iDynInv.cpp:2384
iCub::iDyn::BaseLinkNewtonEuler::getAngAcc
const yarp::sig::Vector & getAngAcc() const
Definition: iDynInv.cpp:875
iCub::iDyn::iDynInvSensor::getSensorForceMoment
yarp::sig::Vector getSensorForceMoment() const
Get the sensor force and moment in a single (6x1) vector.
Definition: iDynInv.cpp:2265
iCub::iDyn::OneChainNewtonEuler::OneChainNewtonEuler
OneChainNewtonEuler(iDyn::iDynChain *_c, std::string _info, const NewEulMode _mode=DYNAMIC, unsigned int verb=iCub::skinDynLib::NO_VERBOSE)
Constructor (note: without FT sensor)
Definition: iDynInv.cpp:1673
iCub::iDyn::BaseLinkNewtonEuler::getIm
double getIm() const
Definition: iDynInv.cpp:893
iCub::skinDynLib::NO_VERBOSE
@ NO_VERBOSE
Definition: common.h:38
iCub::iDyn::SensorLinkNewtonEuler::setTorque
void setTorque(const double _Tau)
Set the OneLink torque, ie the corresponding iDynLink joint torque (nothing in the child classes deri...
Definition: iDynInv.cpp:1428
iCub::iDyn::NewEulMode
NewEulMode
Definition: iDynInv.h:64
iCub::iDyn::OneLinkNewtonEuler::setVerbose
void setVerbose(unsigned int verb=iCub::skinDynLib::VERBOSE)
Set the verbosity level of comments during operations.
Definition: iDynInv.cpp:117
iCub::iDyn::SensorLinkNewtonEuler::getr
const yarp::sig::Vector & getr(bool proj=false)
Definition: iDynInv.cpp:1372
iCub::iDyn::OneChainNewtonEuler::ForwardKinematicFromBase
void ForwardKinematicFromBase()
[classic/inverse] Base function for forward of classical Newton-Euler.
Definition: iDynInv.cpp:1851
iCub::iDyn::BaseLinkNewtonEuler::getRC
const yarp::sig::Matrix & getRC()
Definition: iDynInv.cpp:892
iDyn.h
iCub::iDyn::SensorLinkNewtonEuler::getMass
double getMass() const
Definition: iDynInv.cpp:1364
iCub::iDyn::SensorLinkNewtonEuler::computeLinAcc
void computeLinAcc(iDynLink *link)
Definition: iDynInv.cpp:1517
iCub::iDyn::FinalLinkNewtonEuler::setAngVel
bool setAngVel(const yarp::sig::Vector &_w)
Set the OneLink angular velocity (w), ie the corresponding iDynLink angular velocity (w) (in the chil...
Definition: iDynInv.cpp:1181
iCub::iDyn::FinalLinkNewtonEuler::getInertia
const yarp::sig::Matrix & getInertia() const
Definition: iDynInv.cpp:1143
iCub::iDyn::iDynInvSensor::getTorques
yarp::sig::Vector getTorques() const
Definition: iDynInv.cpp:2142
iCub::iDyn::iDynInvSensorArmNoTorso::getType
std::string getType() const
Definition: iDynInv.cpp:2447
iCub::iDyn::BaseLinkNewtonEuler
Definition: iDynInv.h:491
iCub::iDyn::OneLinkNewtonEuler::getInfo
std::string getInfo() const
Definition: iDynInv.cpp:354
iCub::iDyn::SensorLinkNewtonEuler::getD2q
double getD2q() const
Definition: iDynInv.cpp:1389
iCub::iDyn::SensorLinkNewtonEuler::getRC
const yarp::sig::Matrix & getRC()
Definition: iDynInv.cpp:1370
iCub::iDyn::OneLinkNewtonEuler::getMass
virtual double getMass() const
Definition: iDynInv.cpp:374
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::iDynInvSensorLeg::iDynInvSensorLeg
iDynInvSensorLeg(iDyn::iCubLegDyn *_c, const NewEulMode _mode=DYNAMIC, unsigned int verb=iCub::skinDynLib::NO_VERBOSE)
Constructor: the sensor is automatically set with "right" or "left" choice.
Definition: iDynInv.cpp:2512
iCub::iDyn::SensorLinkNewtonEuler::getAngAccM
const yarp::sig::Vector & getAngAccM() const
Definition: iDynInv.cpp:1392
iCub::iDyn::SensorLinkNewtonEuler::computeLinAccC
void computeLinAccC()
Definition: iDynInv.cpp:1537
iCub::iDyn::iCubLegDyn
Definition: iDyn.h:1451
iCub::iDyn::FinalLinkNewtonEuler::setTorque
void setTorque(const double _Tau)
Set the OneLink torque, ie the corresponding iDynLink joint torque (nothing in the child classes deri...
Definition: iDynInv.cpp:1179
iCub::iDyn::iDynInvSensor::getH
yarp::sig::Matrix getH() const
Get the sensor roto-translational matrix defining its position/orientation wrt the link.
Definition: iDynInv.cpp:2197
iCub::iDyn::iDynInvSensor::setVerbose
void setVerbose(unsigned int verb=iCub::skinDynLib::VERBOSE)
Definition: iDynInv.cpp:2159
iCub::iDyn::SensorLinkNewtonEuler::I
yarp::sig::Matrix I
the semi-link inertia
Definition: iDynInv.h:739
iCub::iDyn::OneLinkNewtonEuler::getLinAccC
virtual const yarp::sig::Vector & getLinAccC() const
Definition: iDynInv.cpp:346
iCub::iDyn::iDynSensor::getMomentsNewtonEuler
yarp::sig::Matrix getMomentsNewtonEuler() const
Returns the links moments as a matrix, where the i-th col is the i-th moment.
Definition: iDynInv.cpp:2692
iCub::iDyn::SensorLinkNewtonEuler::getLinAccC
const yarp::sig::Vector & getLinAccC() const
Definition: iDynInv.cpp:1362
iCub::iDyn::OneLinkNewtonEuler::mode
NewEulMode mode
STATIC/DYNAMIC/DYNAMIC_W_ROTOR/DYNAMIC_CORIOLIS_GRAVITY.
Definition: iDynInv.h:100
iCub::iDyn::iDynSensor::getTorque
double getTorque(const unsigned int iLink) const
Returns the i-th link torque.
Definition: iDynInv.cpp:2688
iCub::iDyn::SensorLinkNewtonEuler::setAngVel
bool setAngVel(const yarp::sig::Vector &_w)
Set the OneLink angular velocity (w), ie the corresponding iDynLink angular velocity (w) (in the chil...
Definition: iDynInv.cpp:1430
iCub::iDyn::OneLinkNewtonEuler::getD2q
virtual double getD2q() const
Definition: iDynInv.cpp:364
iCub::iDyn::FinalLinkNewtonEuler::Mu
yarp::sig::Vector Mu
final moment
Definition: iDynInv.h:622
iCub::iDyn::OneLinkNewtonEuler::getr
virtual const yarp::sig::Vector & getr(bool proj=false)
Definition: iDynInv.cpp:378
iCub::iDyn::FinalLinkNewtonEuler::getAngAccM
const yarp::sig::Vector & getAngAccM() const
Definition: iDynInv.cpp:1129
iCub::ctrl::dot
double dot(const yarp::sig::Matrix &A, int colA, const yarp::sig::Matrix &B, int colB)
iCub::iDyn::OneLinkNewtonEuler::getDq
virtual double getDq() const
Definition: iDynInv.cpp:366
iCub::iDyn::iDynChain::getTorquesNewtonEuler
yarp::sig::Vector getTorquesNewtonEuler() const
Returns the links torque as a vector.
Definition: iDyn.cpp:1257
iCub::iDyn::iDynSensor::getForceMomentEndEff
virtual yarp::sig::Vector getForceMomentEndEff() const
Returns the end-effector force-moment as a single (6x1) vector.
Definition: iDynInv.cpp:2696
iCub::iDyn::SensorLinkNewtonEuler::getKr
double getKr() const
Definition: iDynInv.cpp:1391
iCub::iDyn::OneChainNewtonEuler::BackwardWrenchFromEnd
void BackwardWrenchFromEnd()
[classic] Base function for backward of classical Newton-Euler.
Definition: iDynInv.cpp:1883
iCub::iDyn::iDynSensor::getMoments
yarp::sig::Matrix getMoments() const
Returns the links moments as a matrix, where the i-th col is the i-th moment.
Definition: iDynInv.cpp:2680
iCub::iDyn::FinalLinkNewtonEuler::setMoment
bool setMoment(const yarp::sig::Vector &_Mu)
Set the OneLink moment: either the corresponding iDynLink moment, or the one declared as member in th...
Definition: iDynInv.cpp:1163
iCub::iDyn::OneChainNewtonEuler::BackwardWrenchToBase
bool BackwardWrenchToBase(unsigned int lSens)
[classic/inverse] Base function for inverse Newton-Euler: from the i-th link to the base,...
Definition: iDynInv.cpp:1947
iCub::iDyn::OneChainNewtonEuler::setInfo
void setInfo(const std::string _info)
Definition: iDynInv.cpp:1813
iCub::iDyn::OneLinkNewtonEuler::ForwardKinematics
void ForwardKinematics(OneLinkNewtonEuler *prev)
[Forward Newton-Euler] Compute w, dw, ddp, ddpC, dwM
Definition: iDynInv.cpp:720
iCub::iDyn::SensorLinkNewtonEuler::getAngAcc
const yarp::sig::Vector & getAngAcc() const
Definition: iDynInv.cpp:1358
iCub::iDyn::iDynSensor::getTorques
yarp::sig::Vector getTorques() const
Returns the links torque as a vector.
Definition: iDynInv.cpp:2682
iCub::iDyn::BaseLinkNewtonEuler::getKr
double getKr() const
Definition: iDynInv.cpp:898
iCub::iDyn::BaseLinkNewtonEuler::Tau
double Tau
corresponding torque
Definition: iDynInv.h:509
iCub::iDyn::iDynInvSensor::sens
SensorLinkNewtonEuler * sens
the sensor
Definition: iDynInv.h:1230
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::FinalLinkNewtonEuler::setAsFinal
bool setAsFinal(const yarp::sig::Vector &_F, const yarp::sig::Vector &_Mu)
Set the final frame data.
iCub::iDyn::FinalLinkNewtonEuler::getAngAcc
const yarp::sig::Vector & getAngAcc() const
Definition: iDynInv.cpp:1128
iCub::iDyn::SensorLinkNewtonEuler::ddp
yarp::sig::Vector ddp
linear acceleration
Definition: iDynInv.h:731
iCub::iDyn::OneChainNewtonEuler::nEndEff
unsigned int nEndEff
the index of the end-effector in the chain (the last frame)
Definition: iDynInv.h:895
iCub::iDyn::iDynInvSensor::getSensorMoment
yarp::sig::Vector getSensorMoment() const
Returns the sensor estimated moment.
Definition: iDynInv.cpp:2130
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::BaseLinkNewtonEuler::getLinAcc
const yarp::sig::Vector & getLinAcc() const
Definition: iDynInv.cpp:879
F
F
Definition: compute_ekf_fast.m:22
math.h
iCub::iDyn::iCubArmDyn
Definition: iDyn.h:1327
iCub::iDyn::BaseLinkNewtonEuler::Mu0
yarp::sig::Vector Mu0
initial moment
Definition: iDynInv.h:507
iCub::iDyn::iDynInvSensor::setInfo
void setInfo(const std::string &_info)
Definition: iDynInv.cpp:2165
iCub::iDyn::iDynInvSensor::getSensorLink
unsigned int getSensorLink() const
Definition: iDynInv.cpp:2277
iCub::skinManager::toString
std::string toString(const T &t)
Definition: compensator.h:199
iCub::iDyn::iDynInvSensor::verbose
unsigned int verbose
verbosity flag
Definition: iDynInv.h:1236
iCub::iDyn::iDynSensorArm::iDynSensorArm
iDynSensorArm(iDyn::iCubArmDyn *_c, const NewEulMode _mode=DYNAMIC, unsigned int verb=iCub::skinDynLib::NO_VERBOSE)
Constructor: the sensor is automatically set with "right" or "left" choice.
Definition: iDynInv.cpp:2707
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::OneChainNewtonEuler::toString
std::string toString() const
Useful to print some information.
Definition: iDynInv.cpp:1720
iCub::iDyn::BaseLinkNewtonEuler::BaseLinkNewtonEuler
BaseLinkNewtonEuler(const yarp::sig::Matrix &_H0, const NewEulMode _mode, unsigned int verb=iCub::skinDynLib::NO_VERBOSE)
Default constructor.
iCub::iDyn::SensorLinkNewtonEuler::computeAngVel
void computeAngVel(iDynLink *link)
Definition: iDynInv.cpp:1507
iCub::iDyn::iDynInvSensor::setSensorInfo
void setSensorInfo(const std::string &_info)
Definition: iDynInv.cpp:2170
iCub::iDyn::OneLinkNewtonEuler::BackwardKinematics
void BackwardKinematics(OneLinkNewtonEuler *prev)
[Backward Kinematic computation] Compute w, dw, ddp, ddpC, dwM
Definition: iDynInv.cpp:729
iCub::iDyn::SensorLinkNewtonEuler::setMeasuredFMu
bool setMeasuredFMu(const yarp::sig::Vector &_F, const yarp::sig::Vector &_Mu)
Set the sensor measured force/moment - if measured by a FT sensor.
Definition: iDynInv.cpp:1260
iCub::iDyn::BaseLinkNewtonEuler::getFv
double getFv() const
Definition: iDynInv.cpp:895
iCub::iDyn::iDynInvSensor::setSensor
bool setSensor(unsigned int i, const yarp::sig::Matrix &_H, const yarp::sig::Matrix &_HC, const double _m, const yarp::sig::Matrix &_I)
Set a new sensor or new sensor properties.
iCub::iDyn::OneChainNewtonEuler::getMode
NewEulMode getMode() const
Definition: iDynInv.cpp:1844
iCub::iDyn::FinalLinkNewtonEuler::getTorque
double getTorque() const
Definition: iDynInv.cpp:1132
iCub::iDyn::OneLinkNewtonEuler::computeTorque
virtual void computeTorque(OneLinkNewtonEuler *prev)
[all] Compute joint torque; moment must be pre-computed
Definition: iDynInv.cpp:698
iCub::iDyn::OneChainNewtonEuler::mode
NewEulMode mode
static/dynamic/dynamicWrotor
Definition: iDynInv.h:898
iCub::iDyn::FinalLinkNewtonEuler::getIm
double getIm() const
Definition: iDynInv.cpp:1136
iCub::iDyn::DYNAMIC
@ DYNAMIC
Definition: iDynInv.h:64
iCub::iDyn::SensorLinkNewtonEuler::r_proj
yarp::sig::Vector r_proj
Definition: iDynInv.h:746
iCub::iDyn
Definition: iDyn.h:81
iCub::iDyn::BaseLinkNewtonEuler::dw
yarp::sig::Vector dw
initial angular acceleration
Definition: iDynInv.h:497
iCub::iDyn::OneChainNewtonEuler::nLinks
unsigned int nLinks
number of links
Definition: iDynInv.h:893
iCub::iDyn::STATIC
@ STATIC
Definition: iDynInv.h:64
iCub::iDyn::FinalLinkNewtonEuler::setAngAccM
bool setAngAccM(const yarp::sig::Vector &_dwM)
Set the OneLink angular acceleration of the motor (dwM), ie the corresponding iDynLink angular accele...
Definition: iDynInv.cpp:1213
iCub::iDyn::SensorLinkNewtonEuler::COM
yarp::sig::Matrix COM
the roto-translational matrix of the COM of the semi-link (bewteen sensor and ith link frame)
Definition: iDynInv.h:737
iCub::iDyn::OneLinkNewtonEuler::getLinAcc
virtual const yarp::sig::Vector & getLinAcc() const
Definition: iDynInv.cpp:344
iCub::iKin
Definition: iKinFwd.h:71
iCub::iDyn::OneChainNewtonEuler::ForwardWrenchFromAtoB
bool ForwardWrenchFromAtoB(unsigned int lA, unsigned int lB)
Base function for inverse Newton-Euler: from the lA-th link to the lB-th, forward of forces and momen...
Definition: iDynInv.cpp:1976
iCub::iDyn::FinalLinkNewtonEuler::getR
const yarp::sig::Matrix & getR()
Definition: iDynInv.cpp:1134
iCub::iDyn::FinalLinkNewtonEuler::setLinAcc
bool setLinAcc(const yarp::sig::Vector &_ddp)
Set the OneLink linear acceleration (ddp), ie the corresponding iDynLink linear acceleration (ddp) (i...
Definition: iDynInv.cpp:1198
iCub::iDyn::SensorLinkNewtonEuler
Definition: iDynInv.h:718
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::FinalLinkNewtonEuler::eye3x3
const yarp::sig::Matrix eye3x3
Definition: iDynInv.h:628
iCub::iDyn::SensorLinkNewtonEuler::setAngAccM
bool setAngAccM(const yarp::sig::Vector &_dwM)
Set the OneLink angular acceleration of the motor (dwM), ie the corresponding iDynLink angular accele...
Definition: iDynInv.cpp:1494
mode
GLenum mode
Definition: rendering.cpp:48
iCub::iDyn::OneChainNewtonEuler::chain
iDyn::iDynChain * chain
the real kinematic/dynamic chain of the robot
Definition: iDynInv.h:888
iCub::iDyn::OneLinkNewtonEuler::BackwardWrench
void BackwardWrench(OneLinkNewtonEuler *next)
[Backward Newton-Euler] Compute F, Mu, Tau
Definition: iDynInv.cpp:737
iCub::iDyn::BaseLinkNewtonEuler::getLinAccC
const yarp::sig::Vector & getLinAccC() const
Definition: iDynInv.cpp:881
iCub::iDyn::BaseLinkNewtonEuler::zeros3
const yarp::sig::Vector zeros3
Definition: iDynInv.h:514
iCub::iDyn::OneLinkNewtonEuler::setAsBase
virtual bool setAsBase(const yarp::sig::Vector &_w, const yarp::sig::Vector &_dw, const yarp::sig::Vector &_ddp)
Virtual method to set the frame as the base one: this is useful to initialize the forward phase of Ne...
iCub::iDyn::iDynChain::refLink
iDynLink * refLink(const unsigned int i)
Returns a pointer to the ith iDynLink.
Definition: iDyn.cpp:755
iCub::iDyn::DYNAMIC_CORIOLIS_GRAVITY
@ DYNAMIC_CORIOLIS_GRAVITY
Definition: iDynInv.h:64
iCub::iDyn::iDynInvSensorArmNoTorso::iDynInvSensorArmNoTorso
iDynInvSensorArmNoTorso(iDyn::iCubArmNoTorsoDyn *_c, const NewEulMode _mode=DYNAMIC, unsigned int verb=iCub::skinDynLib::NO_VERBOSE)
Constructor: the sensor is automatically set with "right" or "left" choice.
Definition: iDynInv.cpp:2399
iCub::iDyn::OneLinkNewtonEuler::link
iDyn::iDynLink * link
the corresponding iDynLink
Definition: iDynInv.h:110
iCub::iDyn::BaseLinkNewtonEuler::getR
const yarp::sig::Matrix & getR()
Definition: iDynInv.cpp:891
iCub::iDyn::BaseLinkNewtonEuler::getr
const yarp::sig::Vector & getr(bool proj=false)
Definition: iDynInv.cpp:901
iCub::iDyn::BaseLinkNewtonEuler::setAngAccM
bool setAngAccM(const yarp::sig::Vector &_dwM)
Set the OneLink angular acceleration of the motor (dwM), ie the corresponding iDynLink angular accele...
Definition: iDynInv.cpp:994
iCub::iKin::iKinChain::getN
unsigned int getN() const
Returns the number of Links belonging to the Chain.
Definition: iKinFwd.h:550
iCub::ctrl
Definition: adaptWinPolyEstimator.h:37
iCub::iDyn::OneLinkNewtonEuler::setMode
void setMode(const NewEulMode _mode)
Set the operation mode (static,dynamic etc)
Definition: iDynInv.cpp:122
iCub::iDyn::OneLinkNewtonEuler::getR
virtual const yarp::sig::Matrix & getR()
Definition: iDynInv.cpp:358
iCub::iDyn::SensorLinkNewtonEuler::rc_proj
yarp::sig::Vector rc_proj
Definition: iDynInv.h:748
iCub::iDyn::iDynSensor::getMoment
yarp::sig::Vector getMoment(const unsigned int iLink) const
Returns the i-th link moment.
Definition: iDynInv.cpp:2686
iCub::iDyn::BaseLinkNewtonEuler::toString
std::string toString() const
Useful to print some information.
Definition: iDynInv.cpp:852
iCub::iDyn::FinalLinkNewtonEuler::getFv
double getFv() const
Definition: iDynInv.cpp:1138
iCub::iDyn::SensorLinkNewtonEuler::Mu
yarp::sig::Vector Mu
measured or estimated moment
Definition: iDynInv.h:724
iCub::iDyn::FinalLinkNewtonEuler::setLinAccC
bool setLinAccC(const yarp::sig::Vector &_ddpC)
Set the OneLink linear acceleration of the COM (ddpC), ie the corresponding iDynLink linear accelerat...
Definition: iDynInv.cpp:1206
iCub::iDyn::BaseLinkNewtonEuler::setLinAccC
bool setLinAccC(const yarp::sig::Vector &_ddpC)
Set the OneLink linear acceleration of the COM (ddpC), ie the corresponding iDynLink linear accelerat...
Definition: iDynInv.cpp:987
iCub::iDyn::FinalLinkNewtonEuler::FinalLinkNewtonEuler
FinalLinkNewtonEuler(const yarp::sig::Matrix &_HN, const NewEulMode _mode, unsigned int verb=iCub::skinDynLib::NO_VERBOSE)
Default constructor.
Definition: iDynInv.cpp:1011
iCub::iDyn::BaseLinkNewtonEuler::getForce
const yarp::sig::Vector & getForce() const
Definition: iDynInv.cpp:883
iCub::iDyn::iDynInvSensorLeg::getType
std::string getType() const
Definition: iDynInv.cpp:2558
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::SensorLinkNewtonEuler::R
yarp::sig::Matrix R
Definition: iDynInv.h:743
iCub::iDyn::OneLinkNewtonEuler
Definition: iDynInv.h:95
iCub::iDyn::OneLinkNewtonEuler::info
std::string info
info or useful notes
Definition: iDynInv.h:102
iCub::iDyn::iDynSensorArmNoTorso::getType
std::string getType() const
Definition: iDynInv.cpp:2770
iCub::iDyn::FinalLinkNewtonEuler::zeros3x3
const yarp::sig::Matrix zeros3x3
Definition: iDynInv.h:629
iCub::iDyn::FinalLinkNewtonEuler::ddp
yarp::sig::Vector ddp
initial linear acceleration
Definition: iDynInv.h:618
iCub::iDyn::iDynInvSensor::toString
std::string toString() const
Print some information.
Definition: iDynInv.cpp:2057
iCub::iKin::iKinChain::getH0
yarp::sig::Matrix getH0() const
Returns H0, the rigid roto-translation matrix from the root reference frame to the 0th frame.
Definition: iKinFwd.h:564
iCub::iDyn::SensorLinkNewtonEuler::computeMomentToLink
void computeMomentToLink(iDynLink *link)
Definition: iDynInv.cpp:1570
iCub::iDyn::iDynInvSensor::~iDynInvSensor
virtual ~iDynInvSensor()
Definition: iDynInv.cpp:2047
iCub::iDyn::iDynInvSensor::mode
NewEulMode mode
static/dynamic/etc..
Definition: iDynInv.h:1234
iCub::iDyn::SensorLinkNewtonEuler::getAngVel
const yarp::sig::Vector & getAngVel() const
Definition: iDynInv.cpp:1356
iCub::iDyn::OneLinkNewtonEuler::getKr
virtual double getKr() const
Definition: iDynInv.cpp:368
iCub::iDyn::BaseLinkNewtonEuler::getD2q
double getD2q() const
Definition: iDynInv.cpp:896
iCub::iDyn::BaseLinkNewtonEuler::setLinAcc
bool setLinAcc(const yarp::sig::Vector &_ddp)
Set the OneLink linear acceleration (ddp), ie the corresponding iDynLink linear acceleration (ddp) (i...
Definition: iDynInv.cpp:971
iCub::iDyn::iDynSensor
Definition: iDynInv.h:1577
iCub::iDyn::SensorLinkNewtonEuler::setSensor
bool setSensor(const yarp::sig::Matrix &_H, const yarp::sig::Matrix &_HC, const double _m, const yarp::sig::Matrix &_I)
Set a new sensor or new sensor properties.
Definition: iDynInv.cpp:1280
iCub::iDyn::BaseLinkNewtonEuler::zeros3x3
const yarp::sig::Matrix zeros3x3
Definition: iDynInv.h:513
iCub::iDyn::OneChainNewtonEuler::~OneChainNewtonEuler
~OneChainNewtonEuler()
Standard destructor.
Definition: iDynInv.cpp:1713
iCub::iDyn::iDynInvSensor::chain
iDynChain * chain
the iDynChain describing the robotic chain
Definition: iDynInv.h:1232
iCub::iDyn::FinalLinkNewtonEuler::getr
const yarp::sig::Vector & getr(bool proj=false)
Definition: iDynInv.cpp:1144
iCub::iKin::iKinChain::getHN
yarp::sig::Matrix getHN() const
Returns HN, the rigid roto-translation matrix from the Nth frame to the end-effector.
Definition: iKinFwd.h:579
iCub::iDyn::SensorLinkNewtonEuler::H
yarp::sig::Matrix H
the roto-translational matrix from the i-th link to the sensor: it's the matrix describing the sensor...
Definition: iDynInv.h:735
iCub::iDyn::OneChainNewtonEuler::neChain
OneLinkNewtonEuler ** neChain
the chain of links/frames for Newton-Euler computations
Definition: iDynInv.h:890
iCub::iDyn::SensorLinkNewtonEuler::computeAngAcc
void computeAngAcc(iDynLink *link)
Definition: iDynInv.cpp:1512
iCub::iDyn::iDynInvSensor::setDynamicParameters
bool setDynamicParameters(const double _m, const yarp::sig::Matrix &_HC, const yarp::sig::Matrix &_I)
Set the dynamic parameters of the the portion of link defined between sensor and i-th frame.
Definition: iDynInv.cpp:2175
iCub::iDyn::SensorLinkNewtonEuler::getIm
double getIm() const
Definition: iDynInv.cpp:1386
iCub::iDyn::OneChainNewtonEuler::computeTorques
void computeTorques()
Computes all the torques in the chain, assuming that all the internal wrenches have been already comp...
Definition: iDynInv.cpp:1892
iCub::iDyn::SensorLinkNewtonEuler::setAngAcc
bool setAngAcc(const yarp::sig::Vector &_dw)
Set the OneLink angular acceleration (dw), ie the corresponding iDynLink angular acceleration (dw) (i...
Definition: iDynInv.cpp:1446
iCub::iDyn::SensorLinkNewtonEuler::computeMoment
void computeMoment(iDynLink *link)
Definition: iDynInv.cpp:1612
iCub::iDyn::FinalLinkNewtonEuler::HN
yarp::sig::Matrix HN
final roto-traslation (if necessary)
Definition: iDynInv.h:624
iCub::iDyn::OneLinkNewtonEuler::getAngAccM
virtual const yarp::sig::Vector & getAngAccM() const
Definition: iDynInv.cpp:342
iCub::iDyn::BaseLinkNewtonEuler::getAngAccM
const yarp::sig::Vector & getAngAccM() const
Definition: iDynInv.cpp:877
iCub::iDyn::iDynInvSensor::getCOM
yarp::sig::Matrix getCOM() const
Get the sensor roto-traslational matrix of the center of mass of the semi-link defined by the sensor ...
Definition: iDynInv.cpp:2223
iCub::iDyn::SensorLinkNewtonEuler::w
yarp::sig::Vector w
angular velocity
Definition: iDynInv.h:727
iCub::iDyn::SensorLinkNewtonEuler::getMoment
const yarp::sig::Vector & getMoment(bool isBase=false) const
Definition: iDynInv.cpp:1354
iCub::ctrl::cross
yarp::sig::Vector cross(const yarp::sig::Matrix &A, int colA, const yarp::sig::Matrix &B, int colB)
f
f
Definition: compute_ekf_sym.m:22
iCub::iDyn::SensorLinkNewtonEuler::dw
yarp::sig::Vector dw
angular acceleration
Definition: iDynInv.h:729
iCub::iDyn::FinalLinkNewtonEuler::dw
yarp::sig::Vector dw
initial angular acceleration
Definition: iDynInv.h:616
iCub::iDyn::iDynInvSensor::setMode
void setMode(const NewEulMode _mode=DYNAMIC)
Definition: iDynInv.cpp:2153
iCub::iDyn::OneLinkNewtonEuler::setInfo
void setInfo(const std::string &_info)
Set some information about this OneLink class.
Definition: iDynInv.cpp:256
iCub::iDyn::SensorLinkNewtonEuler::getR
const yarp::sig::Matrix & getR()
Definition: iDynInv.cpp:1368
iCub::iDyn::SensorLinkNewtonEuler::getType
virtual std::string getType() const
Definition: iDynInv.cpp:1658
iCub::iDyn::SensorLinkNewtonEuler::SensorLinkNewtonEuler
SensorLinkNewtonEuler(const NewEulMode _mode, unsigned int verb=iCub::skinDynLib::NO_VERBOSE)
Default constructor.
Definition: iDynInv.cpp:1229
iCub::iDyn::SensorLinkNewtonEuler::computeForceToLink
void computeForceToLink(iDynLink *link)
Definition: iDynInv.cpp:1562
iCub::iDyn::NewEulMode_s
const std::string NewEulMode_s[4]
Definition: iDynInv.h:65
iCub::iDyn::OneLinkNewtonEuler::getInertia
virtual const yarp::sig::Matrix & getInertia() const
Definition: iDynInv.cpp:376
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::BaseLinkNewtonEuler::H0
yarp::sig::Matrix H0
base roto-traslation (if necessary)
Definition: iDynInv.h:501
iCub::iDyn::iDynChain::getTorque
double getTorque(const unsigned int iLink) const
Returns the i-th link torque.
Definition: iDyn.cpp:788
iCub::iDyn::BaseLinkNewtonEuler::setForce
bool setForce(const yarp::sig::Vector &_F)
Set the OneLink force: either the corresponding iDynLink force, or the one declared as member in the ...
Definition: iDynInv.cpp:904
iCub::iDyn::BaseLinkNewtonEuler::getFs
double getFs() const
Definition: iDynInv.cpp:894
iCub::iDyn::FinalLinkNewtonEuler::getFs
double getFs() const
Definition: iDynInv.cpp:1137
iCub::iDyn::iDynSensorLeg::iDynSensorLeg
iDynSensorLeg(iDyn::iCubLegDyn *_c, const NewEulMode _mode=DYNAMIC, unsigned int verb=iCub::skinDynLib::NO_VERBOSE)
Constructor: the sensor is automatically set with "right" or "left" choice.
Definition: iDynInv.cpp:2787
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::FinalLinkNewtonEuler::setForce
bool setForce(const yarp::sig::Vector &_F)
Set the OneLink force: either the corresponding iDynLink force, or the one declared as member in the ...
Definition: iDynInv.cpp:1147
iCub::iDyn::iCubArmNoTorsoDyn
Definition: iDyn.h:1368
iCub::iDyn::iDynSensor::getForces
yarp::sig::Matrix getForces() const
Returns the links forces as a matrix, where the i-th col is the i-th force.
Definition: iDynInv.cpp:2678
iCub::iDyn::SensorLinkNewtonEuler::ForwardAttachToLink
void ForwardAttachToLink(iDynLink *link)
Compute w,dw,ddp,dppC given the reference frame of the link where the sensor is.
Definition: iDynInv.cpp:1326
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::OneLinkNewtonEuler::getMoment
virtual const yarp::sig::Vector & getMoment(bool isBase=false) const
Definition: iDynInv.cpp:350
iCub::iDyn::SensorLinkNewtonEuler::getH
const yarp::sig::Matrix & getH() const
Definition: iDynInv.cpp:1394
iCub::iDyn::SensorLinkNewtonEuler::setMoment
bool setMoment(const yarp::sig::Vector &_Mu)
Set the OneLink moment: either the corresponding iDynLink moment, or the one declared as member in th...
Definition: iDynInv.cpp:1412
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::iDynInvSensor::iDynInvSensor
iDynInvSensor(iDyn::iDynChain *_c, const std::string &_info, const NewEulMode _mode=DYNAMIC, unsigned int verb=iCub::skinDynLib::NO_VERBOSE)
Constructor without FT sensor: the sensor must be set with setSensor()
Definition: iDynInv.cpp:2025
iCub::iDyn::SensorLinkNewtonEuler::getLinAcc
const yarp::sig::Vector & getLinAcc() const
Definition: iDynInv.cpp:1360
iCub::iDyn::SensorLinkNewtonEuler::r
yarp::sig::Vector r
Definition: iDynInv.h:745
iCub::iDyn::SensorLinkNewtonEuler::BackwardAttachToLink
void BackwardAttachToLink(iDynLink *link)
Compute F,Mu given the reference frame of the link where the sensor is.
Definition: iDynInv.cpp:1334
iCub::iDyn::BaseLinkNewtonEuler::eye3x3
const yarp::sig::Matrix eye3x3
Definition: iDynInv.h:512
iCub::iDyn::iDynSensorArmNoTorso::iDynSensorArmNoTorso
iDynSensorArmNoTorso(iDyn::iCubArmNoTorsoDyn *_c, const NewEulMode _mode=DYNAMIC, unsigned int verb=iCub::skinDynLib::NO_VERBOSE)
Constructor: the sensor is automatically set with "right" or "left" choice.
Definition: iDynInv.cpp:2746
iCub::iDyn::SensorLinkNewtonEuler::getInertia
const yarp::sig::Matrix & getInertia() const
Definition: iDynInv.cpp:1366
iCub::iDyn::OneLinkNewtonEuler::ForwardWrench
void ForwardWrench(OneLinkNewtonEuler *prev)
[Inverse Newton-Euler] Compute F, Mu, Tau
Definition: iDynInv.cpp:743
iCub::iDyn::BaseLinkNewtonEuler::Mu
yarp::sig::Vector Mu
initial moment
Definition: iDynInv.h:505
iCub::iDyn::BaseLinkNewtonEuler::F
yarp::sig::Vector F
initial force
Definition: iDynInv.h:503
iCub::iDyn::iDynSensor::getForcesNewtonEuler
yarp::sig::Matrix getForcesNewtonEuler() const
Returns the links forces as a matrix, where the i-th col is the i-th force.
Definition: iDynInv.cpp:2690
iCub::iDyn::FinalLinkNewtonEuler::getKr
double getKr() const
Definition: iDynInv.cpp:1141
iCub::iDyn::SensorLinkNewtonEuler::setLinAcc
bool setLinAcc(const yarp::sig::Vector &_ddp)
Set the OneLink linear acceleration (ddp), ie the corresponding iDynLink linear acceleration (ddp) (i...
Definition: iDynInv.cpp:1462
iCub::iDyn::BaseLinkNewtonEuler::getTorque
double getTorque() const
Definition: iDynInv.cpp:890
iCub::iDyn::OneChainNewtonEuler::BackwardKinematicFromEnd
void BackwardKinematicFromEnd()
[inverse] Base function for forward of classical Newton-Euler.
Definition: iDynInv.cpp:1867
iCub::iDyn::SensorLinkNewtonEuler::getForce
const yarp::sig::Vector & getForce() const
Definition: iDynInv.cpp:1352
fprintf
fprintf(fid,'\n')
iCub::iDyn::OneChainNewtonEuler::getInfo
std::string getInfo() const
Definition: iDynInv.cpp:1843
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::BaseLinkNewtonEuler::setAsBase
bool setAsBase(const yarp::sig::Vector &_w, const yarp::sig::Vector &_dw, const yarp::sig::Vector &_ddp)
Sets the base data.
iCub::iDyn::iDynSensor::computeFromSensorNewtonEuler
virtual void computeFromSensorNewtonEuler()
The main computation method: given the FT sensor measurements, compute forces moments and torques in ...
Definition: iDynInv.cpp:2611
iCub::iDyn::FinalLinkNewtonEuler::zeros3
const yarp::sig::Vector zeros3
Definition: iDynInv.h:630
iCub::iDyn::FinalLinkNewtonEuler::getH
const yarp::sig::Matrix & getH()
Definition: iDynInv.cpp:1133
iCub::iDyn::BaseLinkNewtonEuler::getDq
double getDq() const
Definition: iDynInv.cpp:897
iCub::iDyn::iDynLimb::getType
std::string getType()
Returns the Limb type as a string.
Definition: iDyn.h:1300
iCub::iDyn::BaseLinkNewtonEuler::ddp
yarp::sig::Vector ddp
initial linear acceleration
Definition: iDynInv.h:499
iCub::iDyn::BaseLinkNewtonEuler::getMass
double getMass() const
Definition: iDynInv.cpp:899
iCub::iDyn::iDynSensor::getTorquesNewtonEuler
yarp::sig::Vector getTorquesNewtonEuler() const
Returns the links torque as a vector.
Definition: iDynInv.cpp:2694
iCub::iDyn::FinalLinkNewtonEuler::getLinAcc
const yarp::sig::Vector & getLinAcc() const
Definition: iDynInv.cpp:1130
zeros
zeros(2, 2) eye(2
iCub::iDyn::OneChainNewtonEuler::setMode
void setMode(const NewEulMode _mode)
Definition: iDynInv.cpp:1806
iCub::iDyn::FinalLinkNewtonEuler::getrC
const yarp::sig::Vector & getrC(bool proj=false)
Definition: iDynInv.cpp:1145
iCub::iDyn::iDynSensorArm::getType
std::string getType() const
Definition: iDynInv.cpp:2730
strain::dsp::q15::zero
const Q15 zero
Definition: strain.h:67
iCub::iDyn::FinalLinkNewtonEuler::toString
std::string toString() const
Useful to print some information.
Definition: iDynInv.cpp:1103
iCub::iDyn::iDynInvSensor::lSens
unsigned int lSens
the link where the sensor is attached to
Definition: iDynInv.h:1228
iCub::iDyn::DYNAMIC_W_ROTOR
@ DYNAMIC_W_ROTOR
Definition: iDynInv.h:64
iCub::iDyn::OneLinkNewtonEuler::getZM
yarp::sig::Vector getZM() const
Definition: iDynInv.cpp:336
iCub::iDyn::iDynInvSensor::info
std::string info
a string with useful information if needed
Definition: iDynInv.h:1238
iCub::iDyn::FinalLinkNewtonEuler::F
yarp::sig::Vector F
final force
Definition: iDynInv.h:620
iCub::iDyn::iDynInvSensor::computeSensorForceMoment
void computeSensorForceMoment()
Compute forces and moments at the sensor frame; this method calls special Forward and Backward method...
Definition: iDynInv.cpp:2102
iCub::iDyn::OneChainNewtonEuler::setVerbose
void setVerbose(unsigned int verb=iCub::skinDynLib::VERBOSE)
Definition: iDynInv.cpp:1799
iCub::iDyn::FinalLinkNewtonEuler::eye4x4
const yarp::sig::Matrix eye4x4
Definition: iDynInv.h:627
iCub::iDyn::SensorLinkNewtonEuler::m
double m
the semi-link mass (the portion of link defined by the sensor)
Definition: iDynInv.h:741
iCub::iDyn::SensorLinkNewtonEuler::ForwardForcesMomentsToLink
void ForwardForcesMomentsToLink(iDynLink *link)
Forward the sensor forces and moments, measured by the sensor, to the reference frame of the link whe...
Definition: iDynInv.cpp:1340
iCub::iDyn::SensorLinkNewtonEuler::setForce
bool setForce(const yarp::sig::Vector &_F)
Set the OneLink force: either the corresponding iDynLink force, or the one declared as member in the ...
Definition: iDynInv.cpp:1396
iCub::iDyn::iDynInvSensor::getSensorInfo
std::string getSensorInfo() const
Definition: iDynInv.cpp:2253
iCub::iDyn::OneLinkNewtonEuler::getrC
virtual const yarp::sig::Vector & getrC(bool proj=false)
Definition: iDynInv.cpp:380
iCub::iDyn::iDynSensor::computeWrenchFromSensorNewtonEuler
virtual void computeWrenchFromSensorNewtonEuler()
The main computation method: given the FT sensor measurements, compute forces moments and torques in ...
Definition: iDynInv.cpp:2634
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::SensorLinkNewtonEuler::toString
std::string toString() const
Useful to print some information.
Definition: iDynInv.cpp:1310
iCub::iDyn::iDynInvSensor::getInfo
std::string getInfo() const
Definition: iDynInv.cpp:2195
iCub::iDyn::iDynChain::getTorques
yarp::sig::Vector getTorques() const
Returns the links torque as a vector.
Definition: iDyn.cpp:875
iCub::iDyn::iDynSensorLeg::getType
std::string getType() const
Definition: iDynInv.cpp:2810
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::FinalLinkNewtonEuler::setAngAcc
bool setAngAcc(const yarp::sig::Vector &_dw)
Set the OneLink angular acceleration (dw), ie the corresponding iDynLink angular acceleration (dw) (i...
Definition: iDynInv.cpp:1190
iCub::iDyn::BaseLinkNewtonEuler::setAngVel
bool setAngVel(const yarp::sig::Vector &_w)
Set the OneLink angular velocity (w), ie the corresponding iDynLink angular velocity (w) (in the chil...
Definition: iDynInv.cpp:939
iCub::iDyn::SensorLinkNewtonEuler::ddpC
yarp::sig::Vector ddpC
linear acceleration of the COM
Definition: iDynInv.h:733
iCub::iDyn::OneLinkNewtonEuler::getIm
virtual double getIm() const
Definition: iDynInv.cpp:362
iCub::iDyn::BaseLinkNewtonEuler::getInertia
const yarp::sig::Matrix & getInertia() const
Definition: iDynInv.cpp:900
iCub::iDyn::FinalLinkNewtonEuler
Definition: iDynInv.h:610
iCub::iDyn::iDynSensor::setSensorMeasures
bool setSensorMeasures(const yarp::sig::Vector &F, const yarp::sig::Vector &Mu)
Set the sensor measured force and moment.
iCub::iDyn::SensorLinkNewtonEuler::getrC
const yarp::sig::Vector & getrC(bool proj=false)
Definition: iDynInv.cpp:1379
iCub::iDyn::BaseLinkNewtonEuler::getAngVel
const yarp::sig::Vector & getAngVel() const
Definition: iDynInv.cpp:873
iCub::iDyn::BaseLinkNewtonEuler::setTorque
void setTorque(const double _Tau)
Set the OneLink torque, ie the corresponding iDynLink joint torque (nothing in the child classes deri...
Definition: iDynInv.cpp:937
iCub::iDyn::OneLinkNewtonEuler::setAsFinal
virtual bool setAsFinal(const yarp::sig::Vector &_F, const yarp::sig::Vector &_Mu)
Set the frame as the final one: this is useful to initialize the backward phase of Newton-Euler's met...
iCub::iDyn::BaseLinkNewtonEuler::getrC
const yarp::sig::Vector & getrC(bool proj=false)
Definition: iDynInv.cpp:902
iCub::iDyn::FinalLinkNewtonEuler::getD2q
double getD2q() const
Definition: iDynInv.cpp:1139
iCub::iDyn::OneChainNewtonEuler::ForwardWrenchToEnd
bool ForwardWrenchToEnd(unsigned int lSens)
[inverse] Base function for inverse Newton-Euler: from the i-th link to the end, forward of forces an...
Definition: iDynInv.cpp:1926
iCub::iDyn::FinalLinkNewtonEuler::getMass
double getMass() const
Definition: iDynInv.cpp:1142
iCub::iDyn::SensorLinkNewtonEuler::zeros0
const yarp::sig::Vector zeros0
Definition: iDynInv.h:749
Generated on Wed Jun 29 2022 10:33:41 for iCub-main by   doxygen 1.8.17