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