Go to the documentation of this file.
20 #include <yarp/math/Math.h>
31 using namespace yarp::sig;
32 using namespace yarp::math;
46 OneLinkNewtonEuler::OneLinkNewtonEuler(
iDynLink *dlink)
52 z0.resize(3); z0.
zero(); z0(2)=1;
53 zm.resize(3); zm.zero();
56 OneLinkNewtonEuler::OneLinkNewtonEuler(
const NewEulMode _mode,
unsigned int verb,
iDynLink *dlink)
62 z0.resize(3); z0.
zero(); z0(2)=1;
63 zm.resize(3); zm.zero();
68 z0.resize(3); z0.zero(); z0(2)=1;
69 zm.resize(3); zm.zero();
70 if (link != NULL) link->zero();
79 bool OneLinkNewtonEuler::setAsBase(
const Vector &_w,
const Vector &_dw,
const Vector &_ddp)
89 bool OneLinkNewtonEuler::setAsBase(
const Vector &_F,
const Vector &_Mu)
98 bool OneLinkNewtonEuler::setAsFinal(
const Vector &_F,
const Vector &_Mu)
107 bool OneLinkNewtonEuler::setAsFinal(
const Vector &_w,
const Vector &_dw,
const Vector &_ddp)
117 void OneLinkNewtonEuler::setVerbose(
unsigned int verb)
127 bool OneLinkNewtonEuler::setZM(
const Vector &_zm)
138 fprintf(stderr,
"OneLinkNewtonEuler error: could not set Zm due to wrong size vector: ");
139 fprintf(stderr,
"%d instead of 3. \n",(
int)_zm.length());
146 bool OneLinkNewtonEuler::setForce(
const Vector &_F)
155 if(verbose)
fprintf(stderr,
"OneLink error, could not set force due to wrong size: %d instead of 3.\n",(
int)_F.length());
161 bool OneLinkNewtonEuler::setMoment(
const Vector &_Mu)
170 if(verbose)
fprintf(stderr,
"OneLink error, could not set moment due to wrong size: %d instead of 3.\n",(
int)_Mu.length());
176 void OneLinkNewtonEuler::setTorque(
const double _Tau)
181 bool OneLinkNewtonEuler::setAngVel(
const Vector &_w)
190 if(verbose)
fprintf(stderr,
"OneLink error, could not set w due to wrong size: %d instead of 3. \n",(
int)_w.length());
196 bool OneLinkNewtonEuler::setAngAcc(
const Vector &_dw)
205 if(verbose)
fprintf(stderr,
"OneLink error, could not set dw due to wrong size: %d instead of 3. \n",(
int)_dw.length());
211 bool OneLinkNewtonEuler::setLinAcc(
const Vector &_ddp)
220 if(verbose)
fprintf(stderr,
"OneLink error, could not set ddp due to wrong size: %d instead of 3. \n",(
int)_ddp.length());
226 bool OneLinkNewtonEuler::setLinAccC(
const Vector &_ddpC)
228 if(_ddpC.length()==3)
235 if(verbose)
fprintf(stderr,
"OneLink error, could not set ddpC due to wrong size: %d instead of 3. \n",(
int)_ddpC.length());
241 bool OneLinkNewtonEuler::setAngAccM(
const Vector &_dwM)
250 if(verbose)
fprintf(stderr,
"OneLink error, could not set dwM due to wrong size: %d instead of 3. \n",(
int)_dwM.length());
256 void OneLinkNewtonEuler::setInfo(
const string &_info)
261 bool OneLinkNewtonEuler::setMeasuredFMu(
const Vector &_F,
const Vector &_Mu)
265 if((_F.length()==3)&&(_Mu.length()==3))
267 link->setForceMoment(_F,_Mu);
272 if(verbose)
fprintf(stderr,
"OneLinkNewtonEuler error: could not set forces/moments due to wrong dimensions: (%d,%d) instead of (3,3). \n",(
int)_F.length(),(
int)_Mu.length());
279 if(verbose)
fprintf(stderr,
"OneLinkNewtonEuler error: could not set forces/moments due to missing link. \n");
286 bool OneLinkNewtonEuler::setMeasuredTorque(
const double _Tau)
290 link->setTorque(_Tau);
295 if(verbose)
fprintf(stderr,
"OneLinkNewtonEuler error: could not set torque due to missing link. \n");
305 char buffer[300];
int j=0;
306 j=sprintf(buffer,
" [Torque]: %f",link->Tau);
307 j+=sprintf(buffer+j,
" [Force]: %.3f,%.3f,%.3f",link->F(0),link->F(1),link->F(2));
308 j+=sprintf(buffer+j,
" [Moment]: %.3f,%.3f,%.3f",link->Mu(0),link->Mu(1),link->Mu(2));
311 j+=sprintf(buffer+j,
" [mass]: %.3f",link->m);
312 Vector r = link->getr();
313 j+=sprintf(buffer+j,
" [r]: %.3f,%.3f,%.3f",r(0),r(1),r(2));
315 j+=sprintf(buffer+j,
" [rC]: %.3f,%.3f,%.3f",r(0),r(1),r(2));
319 Matrix I = link->getInertia();
320 j+=sprintf(buffer+j,
" [Inertia]: %.3f, %.3f, %.3f; %.3f, %.3f, %.3f; %.3f, %.3f, %.3f",I(0,0),I(0,1),I(0,2),I(1,0),I(1,1),I(1,2),I(2,0),I(2,1),I(2,2));
336 Vector OneLinkNewtonEuler::getZM()
const {
return zm;}
338 const Vector& OneLinkNewtonEuler::getAngVel()
const {
return link->w;}
340 const Vector& OneLinkNewtonEuler::getAngAcc()
const {
return link->dw;}
342 const Vector& OneLinkNewtonEuler::getAngAccM()
const {
return link->dwM;}
344 const Vector& OneLinkNewtonEuler::getLinAcc()
const {
return link->ddp;}
346 const Vector& OneLinkNewtonEuler::getLinAccC()
const {
return link->ddpC;}
348 const Vector& OneLinkNewtonEuler::getForce()
const {
return link->F;}
350 const Vector& OneLinkNewtonEuler::getMoment(
bool isBase)
const {
return link->Mu;}
352 double OneLinkNewtonEuler::getTorque()
const {
return link->Tau;}
354 string OneLinkNewtonEuler::getInfo()
const {
return info;}
356 const Matrix& OneLinkNewtonEuler::getH() {
return link->getH();}
358 const Matrix& OneLinkNewtonEuler::getR() {
return link->getR();}
360 const Matrix& OneLinkNewtonEuler::getRC() {
return link->getRC();}
362 double OneLinkNewtonEuler::getIm()
const {
return link->Im;}
364 double OneLinkNewtonEuler::getD2q()
const {
return link->ddq;}
366 double OneLinkNewtonEuler::getDq()
const {
return link->dq;}
368 double OneLinkNewtonEuler::getKr()
const {
return link->kr;}
370 double OneLinkNewtonEuler::getFs()
const {
return link->Fs;}
372 double OneLinkNewtonEuler::getFv()
const {
return link->Fv;}
374 double OneLinkNewtonEuler::getMass()
const {
return link->m;}
376 const Matrix& OneLinkNewtonEuler::getInertia()
const {
return link->getInertia();}
378 const Vector& OneLinkNewtonEuler::getr(
bool proj) {
return link->getr(proj);}
380 const Vector& OneLinkNewtonEuler::getrC(
bool proj) {
return link->getrC(proj);}
417 w[2] -= next->
getDq();
437 dw[0] += getDq()*prevW[1];
438 dw[1] -= getDq()*prevW[0];
440 setAngAcc(dw * getR());
448 dw[0] += getDq()*prevW[1];
449 dw[1] -= getDq()*prevW[0];
450 setAngAcc(dw * getR());
468 const Vector& w = getAngVel();
469 nextDw[0] -= next->
getDq()*w[1];
470 nextDw[1] += next->
getDq()*w[0];
471 nextDw[2] -= next->
getD2q();
479 const Vector& w = getAngVel();
480 nextDw[0] -= next->
getDq()*w[1];
481 nextDw[1] += next->
getDq()*w[0];
494 const Matrix& R = getR();
501 const Vector& r = getr(
true);
503 temp +=
cross(link->dw, r);
519 const Matrix& R = next->
getR();
526 const Vector& r = next->
getr(
true);
542 void OneLinkNewtonEuler::computeLinAccC()
550 Vector temp = getLinAcc();
551 temp +=
cross(getAngAcc(),getrC());
552 temp +=
cross(getAngVel(),
cross(getAngVel(),getrC()));
558 setLinAccC( getLinAcc());
572 setAngAccM( prev->
getAngAcc() + (getKr() * getD2q()) * zm
576 setAngAccM(
zeros(3));
585 setForce(next->
getR() * temp);
590 Vector temp = prev->
getForce()*getR();
591 temp -= getMass() * getLinAccC();
598 const Matrix& Rn = next->
getR();
599 const Vector& rnp = next->
getr(
true);
644 const Matrix& R = link->getR();
645 const Vector& RTr = link->getr(
true);
652 temp -=
cross(RTr, link->F);
653 temp -=
cross(RTr + link->rc, link->m * link->ddpC);
654 temp -= link->I * link->dw;
655 temp -=
cross( link->w, link->I * link->w);
667 temp -= getKr() * getD2q() * getIm() * getZM();
668 temp -= getKr() * getDq() * getIm() *
cross(getAngVel(),getZM());
670 temp -=
cross(RTr, getForce());
671 temp -=
cross(RTr + getrC(), getMass() * getLinAccC());
672 temp -= getInertia() * getAngAcc();
673 temp -=
cross( getAngVel(), getInertia()*getAngVel());
688 temp -=
cross(RTr, getForce());
689 temp -=
cross(RTr + getrC(), getMass() * getLinAccC());
708 setTorque(prev->
getMoment(
true)[2] + getKr() * getIm() *
dot(getAngAccM(),zm)
709 + getFv() * getDq() + getFs() * sign(getDq()));
722 this->computeAngVel(prev);
723 this->computeAngAcc(prev);
724 this->computeLinAcc(prev);
725 this->computeLinAccC();
731 this->computeAngVelBackward(prev);
732 this->computeAngAccBackward(prev);
733 this->computeLinAccBackward(prev);
734 this->computeLinAccC();
739 this->computeForceBackward(next);
740 this->computeMomentBackward(next);
745 this->computeForceForward(prev);
746 this->computeMomentForward(prev);
747 this->computeTorque(prev);
759 BaseLinkNewtonEuler::BaseLinkNewtonEuler(
const Matrix &_H0,
const NewEulMode _mode,
unsigned int verb)
763 w.resize(3); w.zero();
764 dw.resize(3); dw.zero();
765 ddp.resize(3); ddp.zero();
766 H0.resize(4,4); H0.eye();
767 if((_H0.rows()==4)&&(_H0.cols()==4))
772 fprintf(stderr,
"BaseLink error, could not set H0 due to wrong dimensions: ( %zu,%zu) instead of (4,4). \n",_H0.rows(),_H0.cols());
774 fprintf(stderr,
" Default is set. \n");
776 F.resize(3);
F.zero();
777 Mu.resize(3); Mu.zero();
785 w.resize(3); w.zero();
786 dw.resize(3); dw.zero();
787 ddp.resize(3); ddp.zero();
788 H0.resize(4,4); H0.eye();
789 if((_H0.rows()==4)&&(_H0.cols()==4))
794 fprintf(stderr,
"BaseLink error, could not set H0 due to wrong dimensions: ( %zu,%zu) instead of (4,4). \n",_H0.rows(),_H0.cols());
796 fprintf(stderr,
" Default is set. \n");
798 F.resize(3);
F.zero();
799 Mu.resize(3); Mu.zero();
806 if((_w.length()==3)&&(_dw.length()==3)&&(_ddp.length()==3))
815 w.resize(3);
w.zero();
816 dw.resize(3);
dw.zero();
817 ddp.resize(3);
ddp.zero();
821 fprintf(stderr,
"BaseLinkNewtonEuler error: could not set w/dw/ddp due to wrong dimensions: (%d,%d,%d) instead of (3,3,3). ",(
int)_w.length(),(
int)_dw.length(),(
int)_ddp.length());
823 fprintf(stderr,
" Default is set. \n");
831 if((_F.length()==3)&&(_Mu.length()==3))
839 F.resize(3);
F.zero();
840 Mu.resize(3);
Mu.zero();
844 fprintf(stderr,
"FinalLinkNewtonEuler error: could not set F/Mu due to wrong dimensions: (%d,%d) instead of (3,3).",(
int)_F.length(),(
int)_Mu.length());
846 fprintf(stderr,
" Default is set. \n");
856 char buffer[300];
int j=0;
859 j=sprintf(buffer,
" [w] : %.3f,%.3f,%.3f",
w(0),
w(1),
w(2));
860 j+=sprintf(buffer+j,
" [dw] : %.3f,%.3f,%.3f",
dw(0),
dw(1),
dw(2));
861 j+=sprintf(buffer+j,
" [ddp]: %.3f,%.3f,%.3f",
ddp(0),
ddp(1),
ddp(2));
908 F=
H0.submatrix(0,2,0,2)*_F;
914 fprintf(stderr,
"BaseLink error, could not set force due to wrong dimension: %d instead of 3.\n",(
int)_F.length());
924 Mu =
H0.submatrix(0,2,0,2)*_Mu;
931 fprintf(stderr,
"BaseLink error, could not set moment due to wrong dimension: %d instead of 3.\n",(
int)_Mu.length());
943 w =
H0.submatrix(0,2,0,2).transposed()*_w;
949 fprintf(stderr,
"BaseLink error, could not set w due to wrong size: %d instead of 3. \n",(
int)_w.length());
959 dw =
H0.submatrix(0,2,0,2).transposed()*_dw;
965 fprintf(stderr,
"BaseLink error, could not set dw due to wrong size: %d instead of 3. \n",(
int)_dw.length());
975 ddp =
H0.submatrix(0,2,0,2).transposed()*_ddp;
981 fprintf(stderr,
"BaseLink error, could not set ddp due to wrong size: %d instead of 3. \n",(
int)_ddp.length());
990 fprintf(stderr,
"BaseLink error: no ddpC existing \n");
997 fprintf(stderr,
"BaseLink error: no dwM existing \n");
1015 F.resize(3);
F.zero();
1016 Mu.resize(3);
Mu.zero();
1017 w.resize(3);
w.zero();
1018 dw.resize(3);
dw.zero();
1019 ddp.resize(3);
ddp.zero();
1020 HN.resize(4,4);
HN.eye();
1021 if((_HN.rows()==4)&&(_HN.cols()==4))
1026 fprintf(stderr,
"BaseLink error, could not set HN due to wrong dimensions: ( %zu,%zu) instead of (4,4). \n",_HN.rows(),_HN.cols());
1028 fprintf(stderr,
" Default is set. \n");
1036 F.resize(3);
F.zero();
1037 Mu.resize(3);
Mu.zero();
1038 w.resize(3);
w.zero();
1039 dw.resize(3);
dw.zero();
1040 ddp.resize(3);
ddp.zero();
1042 HN.resize(4,4);
HN.eye();
1043 if((_HN.rows()==4)&&(_HN.cols()==4))
1048 fprintf(stderr,
"BaseLink error, could not set H0 due to wrong dimensions: ( %zu,%zu) instead of (4,4). \n",_HN.rows(),_HN.cols());
1050 fprintf(stderr,
" Default is set. \n");
1056 if((_w.length()==3)&&(_dw.length()==3)&&(_ddp.length()==3))
1065 w.resize(3);
w.zero();
1066 dw.resize(3);
dw.zero();
1067 ddp.resize(3);
ddp.zero();
1071 fprintf(stderr,
"FinalLinkNewtonEuler error: could not set w/dw/ddp due to wrong dimensions: (%d,%d,%d) instead of (3,3,3).",(
int)_w.length(),(
int)_dw.length(),(
int)_ddp.length());
1073 fprintf(stderr,
" Default is set. \n");
1082 if((_F.length()==3)&&(_Mu.length()==3))
1090 F.resize(3);
F.zero();
1091 Mu.resize(3);
Mu.zero();
1095 fprintf(stderr,
"FinalLinkNewtonEuler error: could not set F/Mu due to wrong dimensions: (%d,%d) instead of (3,3).",(
int)_F.length(),(
int)_Mu.length());
1097 fprintf(stderr,
" Default is set. \n");
1107 char buffer[300];
int j=0;
1110 j=sprintf( buffer,
" [F] : %.3f,%.3f,%.3f",
F(0),
F(1),
F(2));
1111 j+=sprintf(buffer+j,
" [Mu] : %.3f,%.3f,%.3f",
Mu(0),
Mu(1),
Mu(2));
1157 fprintf(stderr,
"FinalLink error, could not set force due to wrong dimension: %d instead of 3.\n",(
int)_F.length());
1173 fprintf(stderr,
"FinalLink error, could not set moment due to wrong dimension: %d instead of 3.\n",(
int)_Mu.length());
1195 dw=_dw;
return true;
1203 ddp=_ddp;
return true;
1209 fprintf(stderr,
"FinalLink error: no ddpC existing \n");
1216 fprintf(stderr,
"FinalLink error: no dwM existing \n");
1233 F.resize(3);
F.zero();
1234 Mu.resize(3);
Mu.zero();
1235 w.resize(3);
w.zero();
1236 dw.resize(3);
dw.zero();
1237 ddp.resize(3);
ddp.zero();
1239 H.resize(4,4);
H.eye();
1240 COM.resize(4,4);
COM.eye();
1243 I.resize(3,3);
I.zero();
1251 F.resize(3);
F.zero();
1252 Mu.resize(3);
Mu.zero();
1253 w.resize(3);
w.zero();
1254 dw.resize(3);
dw.zero();
1255 ddp.resize(3);
ddp.zero();
1262 if((_F.length()==3)&&(_Mu.length()==3))
1270 F.resize(3);
F.zero();
1271 Mu.resize(3);
Mu.zero();
1274 fprintf(stderr,
"SensorLinkNewtonEuler error: could not set F/Mu due to wrong dimensions: (%d,%d) instead of (3,3). Default zero is set.\n",(
int)_F.length(),(
int)_Mu.length());
1282 if((_COM.rows()==4)&&(_COM.cols()==4) && (_H.cols()==4) && (_H.rows()==4) && (_I.rows()==3) && (_I.cols()==3))
1284 H = _H;
R =
H.submatrix(0,2,0,2);
r =
H.subcol(0,3,3);
r_proj =
r*
R;
1293 H.resize(4,4);
H.eye();
1294 COM.resize(4,4);
COM.eye();
1297 I.resize(3,3);
I.zero();
1300 fprintf(stderr,
"SensorLink error, could not set properly H,COM,I due to wrong dimensions: \n");
1301 fprintf(stderr,
" H: (%zu,%zu) instead of (4,4) \n",_H.rows(),_H.cols());
1302 fprintf(stderr,
" COM: (%zu,%zu) instead of (4,4) \n",_COM.rows(),_COM.cols());
1303 fprintf(stderr,
" I: (%zu,%zu) instead of (3,3) \n",_I.rows(),_I.cols());
1304 fprintf(stderr,
"Setting identities and zeros by default.");
1314 char buffer[300];
int j=0;
1317 j=sprintf( buffer,
" [F] : %.3f,%.3f,%.3f",
F(0),
F(1),
F(2));
1318 j+=sprintf(buffer+j,
" [Mu] : %.3f,%.3f,%.3f",
Mu(0),
Mu(1),
Mu(2));
1319 j+=sprintf(buffer+j,
" [R sens] : %.3f,%.3f,%.3f ; %.3f,%.3f,%.3f ; %.3f,%.3f,%.3f ",
H(0,0),
H(0,1),
H(0,2),
H(1,0),
H(1,1),
H(1,2),
H(2,0),
H(2,1),
H(2,2));
1320 j+=sprintf(buffer+j,
" [p sens] : %.3f,%.3f,%.3f",
H(0,3),
H(1,3),
H(2,3));
1406 fprintf(stderr,
"SensorLink error, could not set force due to wrong dimension: %d instead of 3.\n",(
int)_F.length());
1422 fprintf(stderr,
"SensorLink error, could not set moment due to wrong dimension: %d instead of 3.\n",(
int)_Mu.length());
1440 fprintf(stderr,
"SensorLink error, could not set w due to wrong size: %d instead of 3.\n",(
int)_w.length());
1456 fprintf(stderr,
"SensorLink error, could not set dw due to wrong size: %d instead of 3. \n",(
int)_dw.length());
1464 if(_ddp.length()==3)
1472 fprintf(stderr,
"SensorLink error, could not set ddp due to wrong size: %d instead of 3.\n",(
int)_ddp.length());
1480 if(_ddpC.length()==3)
1488 fprintf(stderr,
"SensorLink error, could not set ddp due to wrong size: %d instead of 3.\n",(
int)_ddpC.length());
1497 fprintf(stderr,
"SensorLink error: no dwM existing \n");
1660 return "no type for the basic sensor - only iCub sensors have type";
1679 char buffer[100];
int j=0;
1695 for(
unsigned int i=0; i<
nLinks; i++)
1701 j=sprintf(buffer,
"link <%d>",i);
1702 descript.append(buffer);
1715 for(
unsigned int i=0;i<=
nEndEff;i++)
1723 ret =
"[Chain]: " +
info;
1724 for(
unsigned int i=0;i<=
nEndEff;i++)
1746 fprintf(stderr,
"OneChain error, impossible to retrieve vel/acc due to out of range index: %d > %d \n", i,
nEndEff);
1762 fprintf(stderr,
"OneChain error, impossible to retrieve vel/acc due to out of range index: %d > %d \n",i,
nEndEff);
1802 for(
unsigned int i=0;i<=
nEndEff;i++)
1809 for(
unsigned int i=0;i<=
nEndEff;i++)
1853 for(
unsigned int i=1;i<
nEndEff;i++)
1869 for(
unsigned int i=
nEndEff;i>=1;i--)
1885 for(
int i=
nEndEff-1; i>=0; i--)
1888 for(
int i=
nEndEff-1; i>0; i--)
1894 for(
int i=
nEndEff-1; i>0; i--){
1910 fprintf(stderr,
"ForwardWrenchFromBase: not implemented yet \n");
1919 fprintf(stderr,
"ForwardWrenchFromBase: not implemented yet \n");
1936 for(
unsigned int i=lSens+2; i<
nEndEff; i++)
1942 fprintf(stderr,
"OneChainNewtonEuler error, could not perform ForwardWrenchToEnd because of out of range index: %d >= %d \n",lSens,
nLinks);
1957 for(
int i=lSens; i>=0; i--){
1963 for(
int i=lSens+1; i>0; i--)
1969 fprintf(stderr,
"OneChainNewtonEuler error, could not perform BackwardWrenchToBase because of out of range index: %d >= %d \n",lSens,
nLinks);
1979 if( (lB >= lA) && (lB <=
nLinks))
1982 for(
unsigned int i=lA+1; i<=lB+1; i++){
1990 fprintf(stderr,
"OneChainNewtonEuler error, could not perform ForwardWrenchToEnd because of out of range index: A=%d must be < B=%d, and both < %d \n",lA,lB,
nLinks);
1998 if( (lB <= lA) && (lA<=
nLinks) )
2002 for(
int i=lA+1; i>=(int)(lB+1); i--){
2007 for(
int i=lA+2; i>=(int)(lB+2); i--)
2013 fprintf(stderr,
"OneChainNewtonEuler error, could not perform BackwardWrenchFromAtoB because of out of range index: A=%d must be > B=%d, and both < %d \n",lA,lB,
nLinks);
2028 info = _info +
"chain with sensor: unknown";
2039 info = _info +
"chain with sensor: attached";
2060 ret =
"[Chain]: " +
info +
"\n[Sensor]: yes. ";
2062 int j = sprintf(buffer,
"[before link]: %d",
lSens);
2069 bool iDynInvSensor::setSensor(
unsigned int i,
const Matrix &_H,
const Matrix &_HC,
const double _m,
const Matrix &_I)
2085 fprintf(stderr,
"iDynInvSensor error: could not set FT Sensor inside the dynamic chain due to out of range index: %d >= %d \n",i,
chain->
getN());
2098 fprintf(stderr,
"iDynInvSensor error: could not set FT Sensor inside the dynamic chain due to out of range index: %d >= %d \n",i,
chain->
getN());
2114 fprintf(stderr,
"iDynInvSensor error: could not make computations, the sensor is not set yet.\n");
2125 fprintf(stderr,
"iDynInvSensor error: could not get FT Sensor force, because the sensor is not set yet.\n");
2137 fprintf(stderr,
"iDynInvSensor error: could not get FT Sensor moment, because the sensor is not set yet.\n");
2184 if(
verbose)
fprintf(stderr,
"iDynInvSensor error: could not set dynamic parameters, because the sensor is not set yet.\n");
2204 fprintf(stderr,
"iDynInvSensor error: could not get H of the FT sensor, because the sensor is not set yet.\n");
2217 fprintf(stderr,
"iDynInvSensor error: could not get mass of the FT sensor, because the sensor is not set yet.\n");
2225 Matrix com = Matrix(4,4);
2234 fprintf(stderr,
"iDynInvSensor error: could not get COM of the FT sensor, because the sensor is not set yet.\n");
2247 fprintf(stderr,
"iDynInvSensor error: could not get inertia of the FT sensor, because the sensor is not set yet.\n");
2260 fprintf(stderr,
"iDynInvSensor error: could not get info from the FT sensor, because the sensor is not set yet.\n");
2261 return "FT sensor is not set";
2272 fprintf(stderr,
"iDynInvSensor error: could not get force-moment of the FT sensor, because the sensor is not set yet.\n");
2294 H.resize(4,4);
COM.resize(4,4);
I.resize(3,3);
2297 H.zero();
H(0,0) = 1.0;
H(2,1) = 1.0;
H(1,2) = -1.0;
H(1,3) = 0.08428;
H(3,3) = 1.0;
2298 COM.eye();
COM(0,3) = -1.56e-04;
COM(1,3) = -9.87e-05;
COM(2,3) = 2.98e-2;
2299 I.zero();
I(0,0) = 4.08e-04;
I(0,1) =
I(1,0) = -1.08e-6;
I(0,2) =
I(2,0) = -2.29e-6;
2300 I(1,1) = 3.80e-04;
I(1,2) =
I(2,1) = 3.57e-6;
I(2,2) = 2.60e-4;
2305 if(!(
type ==
"right"))
2308 fprintf(stderr,
"iCubArmSensorLink error: type is not left/right: assuming right.\n");
2312 H.zero();
H(0,0) = -1.0;
H(2,1) = 1.0;
H(1,2) = 1.0;
H(1,3) = -0.08428;
H(3,3) = 1.0;
2313 COM.eye();
COM(0,3) = -1.5906019e-04;
COM(1,3) = 8.2873258e-05;
COM(2,3) = 2.9882773e-02;
2314 I.zero();
I(0,0) = 4.08e-04;
I(0,1) =
I(1,0) = -1.08e-6;
I(0,2) =
I(2,0) = -2.29e-6;
2315 I(1,1) = 3.80e-04;
I(1,2) =
I(2,1) = 3.57e-6;
I(2,2) = 2.60e-4;
2318 R =
H.submatrix(0,2,0,2);
r =
H.subcol(0,3,3);
r_proj =
r*
R;
2348 fprintf(stderr,
"iDynInvSensorArm error: type is not left/right. iCub only has a left and a right arm, it is not an octopus :) \n");
2349 fprintf(stderr,
"iDynInvSensorArm: assuming right arm. \n");
2367 if( !((_type==
"left")||(_type==
"right")) )
2371 fprintf(stderr,
"iDynInvSensorArm error: type is not left/right. iCub only has a left and a right arm, it is not an octopus :) \n");
2372 fprintf(stderr,
"iDynInvSensorArm: assuming right arm.\n");
2410 fprintf(stderr,
"iDynInvSensorArm error: type is not left/right. iCub only has a left and a right arm, it is not an octopus :) \n");
2411 fprintf(stderr,
"iDynInvSensorArm: assuming right arm. \n");
2430 if( !((_type==
"left")||(_type==
"right")) )
2434 fprintf(stderr,
"iDynInvSensorArm error: type is not left/right. iCub only has a left and a right arm, it is not an octopus :) \n");
2435 fprintf(stderr,
"iDynInvSensorArm: assuming right arm. \n");
2466 H.resize(4,4);
COM.resize(4,4);
I.resize(3,3);
2469 H.zero();
H(0,1) = -1.0;
H(1,0) = -1.0;
H(2,2) = -1.0;
H(2,3) = -0.0665;
H(3,3) = 1.0;
2477 if(!(
type ==
"right"))
2480 fprintf(stderr,
"iCubLegSensorLink error: type is not left/right: assuming right. \n");
2485 H.zero();
H(0,1) = 1.0;
H(1,0) = -1.0;
H(2,2) = 1.0;
H(2,3) = 0.0665;
H(3,3) = 1.0;
2491 R =
H.submatrix(0,2,0,2);
r =
H.subcol(0,3,3);
r_proj =
r*
R;
2522 fprintf(stderr,
"iDynInvSensorLeg error: type is not left/right. iCub only has a left and a right leg, it is not a millipede :) \n");
2523 fprintf(stderr,
"iDynInvSensorLeg: assuming right leg.\n");
2541 if( !((_type==
"left")||(_type==
"right")) )
2545 fprintf(stderr,
"iDynInvSensorLeg error: type is not left/right. iCub only has a left and a right leg, it is not a millipede :) \n");
2546 fprintf(stderr,
"iDynInvSensorLeg: assuming right leg.\n");
2591 Vector
f(3);
f[0]=FM[0];
f[1]=FM[1];
f[2]=FM[2];
2592 Vector m(3); m[0]=FM[3];m[1]=FM[4];m[2]=FM[5];
2599 fprintf(stderr,
"iDynSensor error: could not set sensor measures due to wrong sized vector: %d instead of 6 (3+3).\n",(
int)FM.length());
2708 :
iDynSensor(_c->asChain(),_c->getType(),_mode,verb)
2717 fprintf(stderr,
"iDynSensorArm error: type is not left/right. iCub only has a left and a right arm, it is not an octopus :) \n");
2718 fprintf(stderr,
"iDynSensorArm: assuming right arm.\n");
2747 :
iDynSensor(_c->asChain(),_c->getType(),_mode,verb)
2757 fprintf(stderr,
"iDynSensorArm error: type is not left/right. iCub only has a left and a right arm, it is not an octopus :) \n");
2758 fprintf(stderr,
"iDynSensorArm: assuming right arm.\n");
2788 :
iDynSensor(_c->asChain(),_c->getType(),_mode,verb)
2797 fprintf(stderr,
"iDynSensorLeg error: type is not left/right. iCub only has a left and a right leg, it is not a millipede :) \n");
2798 fprintf(stderr,
"iDynSensorLeg: assuming right leg.\n");
OneChainNewtonEuler * NE
pointer to OneChainNewtonEuler class, to be used for computing forces and torques
bool initNewtonEuler(const yarp::sig::Vector &w0, const yarp::sig::Vector &dw0, const yarp::sig::Vector &ddp0, const yarp::sig::Vector &Fend, const yarp::sig::Vector &Muend)
Initialize the Newton-Euler method by setting the base (virtual link) velocity and accelerations ( w0...
const yarp::sig::Vector & getMoment(bool isBase=false) const
unsigned int verbose
verbosity flag
yarp::sig::Vector F
measured or estimated force
yarp::sig::Matrix getForces() const
Returns the links forces as a matrix, where the i-th col is the i-th force.
yarp::sig::Vector w
initial angular velocity
yarp::sig::Matrix getInertia() const
Get the inertia of the portion of link defined between sensor and i-th frame.
virtual const yarp::sig::Vector & getAngVel() const
const yarp::sig::Vector & getdW() const
Get the angular acceleration of the link.
bool setLinAccC(const yarp::sig::Vector &_ddpC)
Set the OneLink linear acceleration of the COM (ddpC), ie the corresponding iDynLink linear accelerat...
yarp::sig::Vector getForceMomentEndEff() const
Returns the end effector force-moment as a single (6x1) vector.
const yarp::sig::Vector & getAngVel() const
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...
const yarp::sig::Vector & getMoment(bool isBase=false) const
const yarp::sig::Vector & getForce() const
const yarp::sig::Vector & getLinAccC() const
iDynSensor(iDyn::iDynChain *_c, std::string _info, const NewEulMode _mode=DYNAMIC, unsigned int verb=iCub::skinDynLib::NO_VERBOSE)
Constructor without FT sensor: the sensor must be set with setSensor()
yarp::sig::Vector getForceMoment() const
Get the sensor force and moment in a single (6x1) vector.
iDynInvSensorArm(iDyn::iCubArmDyn *_c, const NewEulMode _mode=DYNAMIC, unsigned int verb=iCub::skinDynLib::NO_VERBOSE)
Constructor: the sensor is automatically set with "right" or "left" choice.
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
virtual const yarp::sig::Vector & getForce() const
unsigned int verbose
verbosity flag
yarp::sig::Vector w
initial angular velocity
bool BackwardWrenchFromAtoB(unsigned int lA, unsigned int lB)
Base function for inverse Newton-Euler: from the lA-th link to the lB-th, backward of forces and mome...
yarp::sig::Vector getForce(const unsigned int iLink) const
Returns the i-th link force.
bool setMoment(const yarp::sig::Vector &_Mu)
Set the OneLink moment: either the corresponding iDynLink moment, or the one declared as member in th...
bool initWrenchBase(const yarp::sig::Vector &F0, const yarp::sig::Vector &Mu0)
[inverse] Initialize the base with measured or known wrench
std::string info
info or useful notes
void computeForce(iDynLink *link)
const yarp::sig::Matrix & getRC()
void computeKinematicNewtonEuler()
Calls the proper method to compute kinematics variables: either ForwardKinematicFromBase() or Backwar...
yarp::sig::Vector getSensorForce() const
Returns the sensor estimated force.
bool setAngAcc(const yarp::sig::Vector &_dw)
Set the OneLink angular acceleration (dw), ie the corresponding iDynLink angular acceleration (dw) (i...
double getMass() const
Get the mass of the portion of link defined between sensor and i-th frame.
bool initWrenchEnd(const yarp::sig::Vector &F0, const yarp::sig::Vector &Mu0)
[classic] Initialize the end-effector finalLink with measured or known wrench
virtual const yarp::sig::Vector & getAngAcc() const
std::string getType() const
const yarp::sig::Vector & getAngAcc() const
yarp::sig::Vector getSensorForceMoment() const
Get the sensor force and moment in a single (6x1) vector.
OneChainNewtonEuler(iDyn::iDynChain *_c, std::string _info, const NewEulMode _mode=DYNAMIC, unsigned int verb=iCub::skinDynLib::NO_VERBOSE)
Constructor (note: without FT sensor)
void setTorque(const double _Tau)
Set the OneLink torque, ie the corresponding iDynLink joint torque (nothing in the child classes deri...
void setVerbose(unsigned int verb=iCub::skinDynLib::VERBOSE)
Set the verbosity level of comments during operations.
const yarp::sig::Vector & getr(bool proj=false)
void ForwardKinematicFromBase()
[classic/inverse] Base function for forward of classical Newton-Euler.
const yarp::sig::Matrix & getRC()
void computeLinAcc(iDynLink *link)
bool setAngVel(const yarp::sig::Vector &_w)
Set the OneLink angular velocity (w), ie the corresponding iDynLink angular velocity (w) (in the chil...
const yarp::sig::Matrix & getInertia() const
yarp::sig::Vector getTorques() const
std::string getType() const
bool setForce(const yarp::sig::Vector &_F)
Sets the joint force, in the link frame: F^i_i.
std::string getInfo() const
const yarp::sig::Matrix & getRC()
virtual double getMass() const
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
iDynInvSensorLeg(iDyn::iCubLegDyn *_c, const NewEulMode _mode=DYNAMIC, unsigned int verb=iCub::skinDynLib::NO_VERBOSE)
Constructor: the sensor is automatically set with "right" or "left" choice.
const yarp::sig::Vector & getAngAccM() const
void setTorque(const double _Tau)
Set the OneLink torque, ie the corresponding iDynLink joint torque (nothing in the child classes deri...
yarp::sig::Matrix getH() const
Get the sensor roto-translational matrix defining its position/orientation wrt the link.
void setVerbose(unsigned int verb=iCub::skinDynLib::VERBOSE)
yarp::sig::Matrix I
the semi-link inertia
virtual const yarp::sig::Vector & getLinAccC() const
yarp::sig::Matrix getMomentsNewtonEuler() const
Returns the links moments as a matrix, where the i-th col is the i-th moment.
const yarp::sig::Vector & getLinAccC() const
NewEulMode mode
STATIC/DYNAMIC/DYNAMIC_W_ROTOR/DYNAMIC_CORIOLIS_GRAVITY.
double getTorque(const unsigned int iLink) const
Returns the i-th link torque.
bool setAngVel(const yarp::sig::Vector &_w)
Set the OneLink angular velocity (w), ie the corresponding iDynLink angular velocity (w) (in the chil...
virtual double getD2q() const
yarp::sig::Vector Mu
final moment
virtual const yarp::sig::Vector & getr(bool proj=false)
const yarp::sig::Vector & getAngAccM() const
double dot(const yarp::sig::Matrix &A, int colA, const yarp::sig::Matrix &B, int colB)
virtual double getDq() const
yarp::sig::Vector getTorquesNewtonEuler() const
Returns the links torque as a vector.
virtual yarp::sig::Vector getForceMomentEndEff() const
Returns the end-effector force-moment as a single (6x1) vector.
std::string type
the leg type: left/right
void BackwardWrenchFromEnd()
[classic] Base function for backward of classical Newton-Euler.
yarp::sig::Matrix getMoments() const
Returns the links moments as a matrix, where the i-th col is the i-th moment.
double getD2Ang() const
Get the joint acceleration.
bool setMoment(const yarp::sig::Vector &_Mu)
Set the OneLink moment: either the corresponding iDynLink moment, or the one declared as member in th...
bool BackwardWrenchToBase(unsigned int lSens)
[classic/inverse] Base function for inverse Newton-Euler: from the i-th link to the base,...
void zero()
Set all dynamic parameters to zero.
std::string getType() const
void setInfo(const std::string _info)
void ForwardKinematics(OneLinkNewtonEuler *prev)
[Forward Newton-Euler] Compute w, dw, ddp, ddpC, dwM
const yarp::sig::Vector & getAngAcc() const
yarp::sig::Vector getTorques() const
Returns the links torque as a vector.
double Tau
corresponding torque
SensorLinkNewtonEuler * sens
the sensor
void prepareNewtonEuler(const NewEulMode NewEulMode_s=DYNAMIC)
Prepare for the Newton-Euler recursive computation of forces and torques.
bool setAsFinal(const yarp::sig::Vector &_F, const yarp::sig::Vector &_Mu)
Set the final frame data.
const yarp::sig::Vector & getAngAcc() const
yarp::sig::Vector ddp
linear acceleration
unsigned int nEndEff
the index of the end-effector in the chain (the last frame)
yarp::sig::Vector getSensorMoment() const
Returns the sensor estimated moment.
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.
const yarp::sig::Vector & getLinAcc() const
const yarp::sig::Vector & getLinAcc() const
Get the linear acceleration of the link.
yarp::sig::Vector Mu0
initial moment
void setInfo(const std::string &_info)
unsigned int getSensorLink() const
iCubArmSensorLink(const std::string &_type, const NewEulMode _mode=DYNAMIC, unsigned int verb=iCub::skinDynLib::NO_VERBOSE)
Constructor: the sensor parameters are automatically set with "right" or "left" choice.
std::string toString(const T &t)
unsigned int verbose
verbosity flag
const yarp::sig::Vector & getForce() const
Get the link force.
iDynSensorArm(iDyn::iCubArmDyn *_c, const NewEulMode _mode=DYNAMIC, unsigned int verb=iCub::skinDynLib::NO_VERBOSE)
Constructor: the sensor is automatically set with "right" or "left" choice.
yarp::sig::Matrix getForcesNewtonEuler() const
Returns the links forces as a matrix, where the (i+1)-th col is the i-th force.
std::string toString() const
Useful to print some information.
BaseLinkNewtonEuler(const yarp::sig::Matrix &_H0, const NewEulMode _mode, unsigned int verb=iCub::skinDynLib::NO_VERBOSE)
Default constructor.
void computeAngVel(iDynLink *link)
void setSensorInfo(const std::string &_info)
void BackwardKinematics(OneLinkNewtonEuler *prev)
[Backward Kinematic computation] Compute w, dw, ddp, ddpC, dwM
bool setMeasuredFMu(const yarp::sig::Vector &_F, const yarp::sig::Vector &_Mu)
Set the sensor measured force/moment - if measured by a FT sensor.
bool setSensor(unsigned int i, const yarp::sig::Matrix &_H, const yarp::sig::Matrix &_HC, const double _m, const yarp::sig::Matrix &_I)
Set a new sensor or new sensor properties.
NewEulMode getMode() const
virtual void computeTorque(OneLinkNewtonEuler *prev)
[all] Compute joint torque; moment must be pre-computed
NewEulMode mode
static/dynamic/dynamicWrotor
yarp::sig::Vector dw
initial angular acceleration
unsigned int nLinks
number of links
bool setAngAccM(const yarp::sig::Vector &_dwM)
Set the OneLink angular acceleration of the motor (dwM), ie the corresponding iDynLink angular accele...
yarp::sig::Matrix COM
the roto-translational matrix of the COM of the semi-link (bewteen sensor and ith link frame)
virtual const yarp::sig::Vector & getLinAcc() const
bool ForwardWrenchFromAtoB(unsigned int lA, unsigned int lB)
Base function for inverse Newton-Euler: from the lA-th link to the lB-th, forward of forces and momen...
const yarp::sig::Matrix & getR()
bool setLinAcc(const yarp::sig::Vector &_ddp)
Set the OneLink linear acceleration (ddp), ie the corresponding iDynLink linear acceleration (ddp) (i...
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...
const yarp::sig::Matrix eye3x3
bool setAngAccM(const yarp::sig::Vector &_dwM)
Set the OneLink angular acceleration of the motor (dwM), ie the corresponding iDynLink angular accele...
iDyn::iDynChain * chain
the real kinematic/dynamic chain of the robot
void BackwardWrench(OneLinkNewtonEuler *next)
[Backward Newton-Euler] Compute F, Mu, Tau
const yarp::sig::Vector & getLinAccC() const
const yarp::sig::Vector zeros3
virtual bool setAsBase(const yarp::sig::Vector &_w, const yarp::sig::Vector &_dw, const yarp::sig::Vector &_ddp)
Virtual method to set the frame as the base one: this is useful to initialize the forward phase of Ne...
iDynLink * refLink(const unsigned int i)
Returns a pointer to the ith iDynLink.
@ DYNAMIC_CORIOLIS_GRAVITY
iDynInvSensorArmNoTorso(iDyn::iCubArmNoTorsoDyn *_c, const NewEulMode _mode=DYNAMIC, unsigned int verb=iCub::skinDynLib::NO_VERBOSE)
Constructor: the sensor is automatically set with "right" or "left" choice.
iDyn::iDynLink * link
the corresponding iDynLink
const yarp::sig::Matrix & getR()
const yarp::sig::Vector & getr(bool proj=false)
bool setAngAccM(const yarp::sig::Vector &_dwM)
Set the OneLink angular acceleration of the motor (dwM), ie the corresponding iDynLink angular accele...
unsigned int getN() const
Returns the number of Links belonging to the Chain.
void setMode(const NewEulMode _mode)
Set the operation mode (static,dynamic etc)
virtual const yarp::sig::Matrix & getR()
yarp::sig::Vector rc_proj
yarp::sig::Vector getMoment(const unsigned int iLink) const
Returns the i-th link moment.
std::string toString() const
Useful to print some information.
yarp::sig::Vector Mu
measured or estimated moment
bool setLinAccC(const yarp::sig::Vector &_ddpC)
Set the OneLink linear acceleration of the COM (ddpC), ie the corresponding iDynLink linear accelerat...
bool setLinAccC(const yarp::sig::Vector &_ddpC)
Set the OneLink linear acceleration of the COM (ddpC), ie the corresponding iDynLink linear accelerat...
FinalLinkNewtonEuler(const yarp::sig::Matrix &_HN, const NewEulMode _mode, unsigned int verb=iCub::skinDynLib::NO_VERBOSE)
Default constructor.
const yarp::sig::Vector & getForce() const
std::string getType() const
const yarp::sig::Vector & getMoment(const unsigned int iLink) const
Returns the i-th link moment.
std::string info
info or useful notes
std::string getType() const
const yarp::sig::Matrix zeros3x3
yarp::sig::Vector ddp
initial linear acceleration
std::string toString() const
Print some information.
yarp::sig::Matrix getH0() const
Returns H0, the rigid roto-translation matrix from the root reference frame to the 0th frame.
void computeMomentToLink(iDynLink *link)
NewEulMode mode
static/dynamic/etc..
const yarp::sig::Vector & getAngVel() const
virtual double getKr() const
bool setLinAcc(const yarp::sig::Vector &_ddp)
Set the OneLink linear acceleration (ddp), ie the corresponding iDynLink linear acceleration (ddp) (i...
bool setSensor(const yarp::sig::Matrix &_H, const yarp::sig::Matrix &_HC, const double _m, const yarp::sig::Matrix &_I)
Set a new sensor or new sensor properties.
const yarp::sig::Matrix zeros3x3
~OneChainNewtonEuler()
Standard destructor.
iDynChain * chain
the iDynChain describing the robotic chain
const yarp::sig::Vector & getr(bool proj=false)
yarp::sig::Matrix getHN() const
Returns HN, the rigid roto-translation matrix from the Nth frame to the end-effector.
yarp::sig::Matrix H
the roto-translational matrix from the i-th link to the sensor: it's the matrix describing the sensor...
OneLinkNewtonEuler ** neChain
the chain of links/frames for Newton-Euler computations
void computeAngAcc(iDynLink *link)
bool setDynamicParameters(const double _m, const yarp::sig::Matrix &_HC, const yarp::sig::Matrix &_I)
Set the dynamic parameters of the the portion of link defined between sensor and i-th frame.
void computeTorques()
Computes all the torques in the chain, assuming that all the internal wrenches have been already comp...
iCubLegSensorLink(const std::string _type, const NewEulMode _mode=DYNAMIC, unsigned int verb=iCub::skinDynLib::NO_VERBOSE)
Constructor: the sensor parameters are automatically set with "right" or "left" choice.
bool setAngAcc(const yarp::sig::Vector &_dw)
Set the OneLink angular acceleration (dw), ie the corresponding iDynLink angular acceleration (dw) (i...
void computeMoment(iDynLink *link)
yarp::sig::Matrix HN
final roto-traslation (if necessary)
virtual const yarp::sig::Vector & getAngAccM() const
const yarp::sig::Vector & getAngAccM() const
yarp::sig::Matrix getCOM() const
Get the sensor roto-traslational matrix of the center of mass of the semi-link defined by the sensor ...
yarp::sig::Vector w
angular velocity
const yarp::sig::Vector & getMoment(bool isBase=false) const
yarp::sig::Vector cross(const yarp::sig::Matrix &A, int colA, const yarp::sig::Matrix &B, int colB)
yarp::sig::Vector dw
angular acceleration
yarp::sig::Vector dw
initial angular acceleration
void setMode(const NewEulMode _mode=DYNAMIC)
void setInfo(const std::string &_info)
Set some information about this OneLink class.
const yarp::sig::Matrix & getR()
virtual std::string getType() const
SensorLinkNewtonEuler(const NewEulMode _mode, unsigned int verb=iCub::skinDynLib::NO_VERBOSE)
Default constructor.
void computeForceToLink(iDynLink *link)
const std::string NewEulMode_s[4]
virtual const yarp::sig::Matrix & getInertia() const
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...
yarp::sig::Matrix H0
base roto-traslation (if necessary)
double getTorque(const unsigned int iLink) const
Returns the i-th link torque.
bool setForce(const yarp::sig::Vector &_F)
Set the OneLink force: either the corresponding iDynLink force, or the one declared as member in the ...
iDynSensorLeg(iDyn::iCubLegDyn *_c, const NewEulMode _mode=DYNAMIC, unsigned int verb=iCub::skinDynLib::NO_VERBOSE)
Constructor: the sensor is automatically set with "right" or "left" choice.
double getDAng() const
Get the joint velocity.
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.
bool setForce(const yarp::sig::Vector &_F)
Set the OneLink force: either the corresponding iDynLink force, or the one declared as member in the ...
std::string type
the arm type: left/right
yarp::sig::Matrix getForces() const
Returns the links forces as a matrix, where the i-th col is the i-th force.
void ForwardAttachToLink(iDynLink *link)
Compute w,dw,ddp,dppC given the reference frame of the link where the sensor is.
const yarp::sig::Vector & getForce(const unsigned int iLink) const
Returns the i-th link force.
virtual const yarp::sig::Vector & getMoment(bool isBase=false) const
const yarp::sig::Matrix & getH() const
bool setMoment(const yarp::sig::Vector &_Mu)
Set the OneLink moment: either the corresponding iDynLink moment, or the one declared as member in th...
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...
iDynInvSensor(iDyn::iDynChain *_c, const std::string &_info, const NewEulMode _mode=DYNAMIC, unsigned int verb=iCub::skinDynLib::NO_VERBOSE)
Constructor without FT sensor: the sensor must be set with setSensor()
const yarp::sig::Vector & getLinAcc() const
void BackwardAttachToLink(iDynLink *link)
Compute F,Mu given the reference frame of the link where the sensor is.
const yarp::sig::Matrix eye3x3
iDynSensorArmNoTorso(iDyn::iCubArmNoTorsoDyn *_c, const NewEulMode _mode=DYNAMIC, unsigned int verb=iCub::skinDynLib::NO_VERBOSE)
Constructor: the sensor is automatically set with "right" or "left" choice.
const yarp::sig::Matrix & getInertia() const
void ForwardWrench(OneLinkNewtonEuler *prev)
[Inverse Newton-Euler] Compute F, Mu, Tau
yarp::sig::Vector Mu
initial moment
yarp::sig::Vector F
initial force
yarp::sig::Matrix getForcesNewtonEuler() const
Returns the links forces as a matrix, where the i-th col is the i-th force.
bool setLinAcc(const yarp::sig::Vector &_ddp)
Set the OneLink linear acceleration (ddp), ie the corresponding iDynLink linear acceleration (ddp) (i...
void BackwardKinematicFromEnd()
[inverse] Base function for forward of classical Newton-Euler.
const yarp::sig::Vector & getForce() const
std::string getInfo() const
void ForwardWrenchFromBase()
[inverse] Base function for inverse Newton-Euler: from the i-th link to the end, forward of forces an...
bool setAsBase(const yarp::sig::Vector &_w, const yarp::sig::Vector &_dw, const yarp::sig::Vector &_ddp)
Sets the base data.
virtual void computeFromSensorNewtonEuler()
The main computation method: given the FT sensor measurements, compute forces moments and torques in ...
const yarp::sig::Vector zeros3
const yarp::sig::Matrix & getH()
std::string getType()
Returns the Limb type as a string.
yarp::sig::Vector ddp
initial linear acceleration
bool setMoment(const yarp::sig::Vector &_Mu)
Sets the joint moment, in the link frame: Mu^i_i.
yarp::sig::Vector getTorquesNewtonEuler() const
Returns the links torque as a vector.
const yarp::sig::Vector & getLinAcc() const
void setMode(const NewEulMode _mode)
const yarp::sig::Vector & getrC(bool proj=false)
std::string getType() const
std::string getType() const
std::string toString() const
Useful to print some information.
unsigned int lSens
the link where the sensor is attached to
yarp::sig::Vector getZM() const
std::string info
a string with useful information if needed
yarp::sig::Vector F
final force
void computeSensorForceMoment()
Compute forces and moments at the sensor frame; this method calls special Forward and Backward method...
void setVerbose(unsigned int verb=iCub::skinDynLib::VERBOSE)
const yarp::sig::Matrix eye4x4
double m
the semi-link mass (the portion of link defined by the sensor)
void ForwardForcesMomentsToLink(iDynLink *link)
Forward the sensor forces and moments, measured by the sensor, to the reference frame of the link whe...
bool setForce(const yarp::sig::Vector &_F)
Set the OneLink force: either the corresponding iDynLink force, or the one declared as member in the ...
std::string getSensorInfo() const
virtual const yarp::sig::Vector & getrC(bool proj=false)
virtual void computeWrenchFromSensorNewtonEuler()
The main computation method: given the FT sensor measurements, compute forces moments and torques in ...
yarp::sig::Matrix getMoments() const
Returns the links moments as a matrix, where the i-th col is the i-th moment.
std::string toString() const
Useful to print some information.
std::string getInfo() const
yarp::sig::Vector getTorques() const
Returns the links torque as a vector.
std::string getType() const
yarp::sig::Matrix getMomentsNewtonEuler() const
Returns the links moments as a matrix, where the (i+1)-th col is the i-th moment.
const yarp::sig::Vector & getW() const
Get the angular velocity of the link.
bool setAngAcc(const yarp::sig::Vector &_dw)
Set the OneLink angular acceleration (dw), ie the corresponding iDynLink angular acceleration (dw) (i...
bool setAngVel(const yarp::sig::Vector &_w)
Set the OneLink angular velocity (w), ie the corresponding iDynLink angular velocity (w) (in the chil...
yarp::sig::Vector ddpC
linear acceleration of the COM
virtual double getIm() const
const yarp::sig::Matrix & getInertia() const
bool setSensorMeasures(const yarp::sig::Vector &F, const yarp::sig::Vector &Mu)
Set the sensor measured force and moment.
const yarp::sig::Vector & getrC(bool proj=false)
const yarp::sig::Vector & getAngVel() const
void setTorque(const double _Tau)
Set the OneLink torque, ie the corresponding iDynLink joint torque (nothing in the child classes deri...
const yarp::sig::Vector & getMoment() const
Get the link moment.
virtual bool setAsFinal(const yarp::sig::Vector &_F, const yarp::sig::Vector &_Mu)
Set the frame as the final one: this is useful to initialize the backward phase of Newton-Euler's met...
const yarp::sig::Vector & getrC(bool proj=false)
bool ForwardWrenchToEnd(unsigned int lSens)
[inverse] Base function for inverse Newton-Euler: from the i-th link to the end, forward of forces an...
const yarp::sig::Vector zeros0