24 #include <yarp/os/Log.h>
30 using namespace yarp::os;
32 using namespace yarp::sig;
33 using namespace yarp::math;
42 #define NO_JOINT_COSTRAINTS
56 if(verbose) yError(
"iDyn: error: not implemented \n");
61 if(verbose) yError(
"iDyn: error: not implemented \n %s \n",msg.c_str());
66 if(verbose) yError(
"iDyn: warning: this method/class is still under development. Please do not use it! \n %s \n",msg.c_str());
71 w.resize(6); w.zero();
72 if((
f.length()==3)||(m.length()==3))
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];
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());
87 Vector w(6); w.zero();
88 if((
f.length()==3)||(m.length()==3))
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];
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());
102 f.resize(3);
f.zero();
103 m.resize(3); m.zero();
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];
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());
128 iDynLink::iDynLink(
double _A,
double _D,
double _Alpha,
double _Offset,
double _Min,
double _Max)
129 :
iKinLink( _A, _D, _Alpha, _Offset, _Min, _Max)
134 iDynLink::iDynLink(
const double _m,
const Matrix &_HC,
const Matrix &_I,
double _A,
double _D,
double _Alpha,
double _Offset,
double _Min,
double _Max)
135 :
iKinLink( _A, _D, _Alpha, _Offset, _Min, _Max)
143 iDynLink::iDynLink(
const double _m,
const Vector &_C,
const Matrix &_I,
double _A,
double _D,
double _Alpha,
double _Offset,
double _Min,
double _Max)
144 :
iKinLink( _A, _D, _Alpha, _Offset, _Min, _Max)
152 iDynLink::iDynLink(
const double _m,
const double _rCx,
const double _rCy,
const double _rCz,
const double Ixx,
const double Ixy,
const double Ixz,
const double Iyy,
const double Iyz,
const double Izz,
double _A,
double _D,
double _Alpha,
double _Offset,
double _Min,
double _Max)
153 :
iKinLink( _A, _D, _Alpha, _Offset, _Min, _Max)
209 bool iDynLink::setDynamicParameters(
const double _m,
const yarp::sig::Matrix &_HC,
const yarp::sig::Matrix &_I,
const double _kr,
const double _Fv,
const double _Fs,
const double _Im)
233 if( (_I.rows()==3)&&(_I.cols()==3) )
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());
246 void iDynLink::setInertia(
const double Ixx,
const double Ixy,
const double Ixz,
const double Iyy,
const double Iyz,
const double Izz)
248 I.resize(3,3);
I.zero();
250 I(0,1) =
I(1,0) = Ixy;
251 I(0,2) =
I(2,0) = Ixz;
253 I(1,2) =
I(2,1) = Iyz;
265 return iKinLink::setAng(_teta);
289 if((_HC.rows()==4) && (_HC.cols()==4))
292 RC =
HC.submatrix(0,2,0,2);
293 rc =
HC.getCol(3).subVector(0,2);
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());
324 yError(
"iDynLink error, cannot set distance from COM due to wrong sized vector: %d instead of 3 \n",(
int)_rC.length());
336 rc = cat(_rCx, _rCy, _rCz);
350 yError(
"iDynLink error: cannot set forces due to wrong size: %d instead of 3.\n",(
int)_F.length());
365 yError(
"iDynLink error, cannot set moments due to wrong size: %d instead of 3. \n",(
int)_Mu.length());
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();
392 F.resize(3);
F.zero();
393 Mu.resize(3);
Mu.zero();
395 Im = 0.0;
kr = 0.0;
Fv = 0.0;
Fs = 0.0;
431 H_store = iKinLink::getH(
true);
510 iKinChain::dispose();
534 yError(
"setAng() failed since DOF==0\n");
539 int sz=(q.length()>(int)
DOF)?
DOF:q.length();
540 for (
int i=0; i<sz; i++)
553 if(dq.length()>=(
int)
DOF)
555 for(
unsigned int i=0; i<
DOF; i++)
560 yError(
"iDynChain error: setVel() failed: %d joint angles needed \n",
DOF);
572 if(ddq.length()>=(
int)
DOF)
574 for(
unsigned int i=0; i<
DOF; i++)
579 yError(
"iDynChain error: setD2Ang() failed: %d joint angles needed \n",
DOF);
591 for(
unsigned int i=0; i<
DOF; i++)
604 for(
unsigned int i=0; i<
DOF; i++)
615 Vector jointBounds(
DOF);
616 for(
unsigned int i=0; i<
DOF; i++)
626 Vector jointBounds(
DOF);
627 for(
unsigned int i=0; i<
DOF; i++)
639 yError(
"setAng() failed due to out of range index: %d>=%d\n",i,
N);
647 return allList[i]->setDAng(_dq);
650 if(
verbose) yError(
"iDynChain error: setVel() failed due to out of range index: %d >= %d \n",i,
N);
659 return allList[i]->setD2Ang(_ddq);
662 if(
verbose) yError(
"iDynChain error: setD2Ang() failed due to out of range index: %d >= %d \n",i,
N);
673 if(
verbose) yError(
"iDynChain error: getDAng() failed due to out of range index: %d >= %d \n",i,
N);
684 if(
verbose) yError(
"iDynChain error: getD2Ang() failed due to out of range index: %d >= %d \n",i,
N);
692 return allList[i]->getLinVel();
695 if(
verbose) yError(
"iDynChain error: getLinVel() failed due to out of range index: %d >= %d \n",i,
N);
703 return allList[i]->getLinVelC();
706 if(
verbose) yError(
"iDynChain error: getLinVelCOM() failed due to out of range index: %d >= %d \n",i,
N);
714 return allList[i]->getLinAcc();
717 if(
verbose) yError(
"iDynChain error: getLinAcc() failed due to out of range index: %d >= %d \n",i,
N);
725 return allList[i]->getLinAccC();
728 if(
verbose) yError(
"iDynChain error: getLinAccCOM() failed due to out of range index: %d >= %d \n",i,
N);
739 if(
verbose) yError(
"iDynChain error: getAngVel() failed due to out of range index: %d >= %d \n",i,
N);
750 if(
verbose) yError(
"iDynChain error: getAngAcc() failed due to out of range index: %d >= %d \n",i,
N);
761 if(
verbose) yError(
"iDynChain error: refLink() failed due to out of range index: %d >= %d \n",i,
N);
769 return allList[iLink]->getForce();
772 if(
verbose) yError(
"iDynChain error: getForce() failed due to out of range index: %d >= %d \n",iLink,
N);
780 return allList[iLink]->getMoment();
783 if(
verbose) yError(
"iDynChain error: getMoment() failed due to out of range index: %d >= %d \n",iLink,
N);
791 return allList[iLink]->getTorque();
794 if(
verbose) yError(
"iDynChain error: getTorque() failed due to out of range index: %d >= %d \n",iLink,
N);
801 Vector ret(
N); ret.zero();
802 for(
unsigned int i=0;i<
N;i++)
811 for(
unsigned int i=0; i<
N; i++)
817 if(
verbose) yError(
"iDynChain error: setMasses() failed due to wrong vector size: %d instead of %d",(
int)_m.length(),
N);
828 if(
verbose) yError(
"iDynChain error: getMass() failed due to out of range index: %d >= %d \n",i,
N);
842 if(
verbose) yError(
"iDynChain error: setMass() failed due to out of range index: %d >= %d \n",i,
N);
850 return allList[i]->getInertia();
854 yError(
"iDynChain: getInertia() failed due to out of range index: %d >= %d \n",i,
N);
862 for(
unsigned int i=0;i<
N;i++)
863 ret.setCol(i,
allList[i]->getForce());
870 for(
unsigned int i=0;i<
N;i++)
871 ret.setCol(i,
allList[i]->getMoment());
878 for(
unsigned int i=0;i<
N;i++)
883 bool iDynChain::setDynamicParameters(
const unsigned int i,
const double _m,
const Matrix &_HC,
const Matrix &_I,
const double _kr,
const double _Fv,
const double _Fs,
const double _Im)
886 return allList[i]->setDynamicParameters(_m,_HC,_I,_kr,_Fv,_Fs,_Im);
889 if(
verbose) yError(
"iDynChain error: setDynamicParameters() failed due to out of range index: %d >= %d \n",i,
N);
897 return allList[i]->setDynamicParameters(_m,_HC,_I);
900 if(
verbose) yError(
"iDynChain error: setDynamicParameters() failed due to out of range index: %d >= %d \n",i,
N);
908 return allList[i]->setStaticParameters(_m,_HC);
911 if(
verbose) yError(
"iDynChain error: setStaticParameters() failed due to out of range index: %d >= %d \n",i,
N);
921 int j = sprintf(buffer,
"DOF=%d N=%d",
DOF,
N);
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");
941 if((w0.length()==3)&&(dw0.length()==3)&&(ddp0.length()==3)&&(F0.length()==3)&&(Mu0.length()==3))
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());
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");
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");
1022 w.resize(3); dw.resize(3); ddp.resize(3); w=dw=ddp=0.0;
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");
1050 Vector dwM(3);dwM=0.0;
1051 Vector ddpC(3);ddpC=0.0;
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");
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");
1085 F.resize(3); Mu.resize(3);
F=Mu=0.0;
1105 bool iDynChain::initNewtonEuler(
const Vector &w0,
const Vector &dw0,
const Vector &ddp0,
const Vector &Fend,
const Vector &Muend)
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");
1117 if((w0.length()==3)&&(dw0.length()==3)&&(ddp0.length()==3)&&(Fend.length()==3)&&(Muend.length()==3))
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());
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");
1156 if((w0.length()==3)&&(dw0.length()==3)&&(ddp0.length()==3))
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());
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");
1186 if((Fend.length()==3)&&(Muend.length()==3))
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());
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");
1228 Matrix ret(3,
N+2); ret.zero();
1231 for(
unsigned int i=0;i<
N+2;i++)
1236 if(
verbose) yError(
"iDynChain error: trying to call getForcesNewtonEuler() without having prepared Newton-Euler. \n");
1244 Matrix ret(3,
N+2); ret.zero();
1247 for(
unsigned int i=0;i<
N+2;i++)
1252 if(
verbose) yError(
"iDynChain error: trying to call getMomentsNewtonEuler() without having prepared Newton-Euler. \n");
1259 Vector ret(
N); ret.zero();
1262 for(
unsigned int i=0;i<
N;i++)
1267 if(
verbose) yError(
"iDynChain error: trying to call getTorquesNewtonEuler() without having prepared Newton-Euler. \n");
1274 Vector ret(6); ret.zero();
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];
1315 if(
verbose) yError(
"iDynChain error: in setIterMode() could not set iteration mode due to unexisting mode \n");
1329 if(
verbose) yError(
"iDynChain: computeGeoJacobian() failed since DOF==0 \n");
1334 if(
verbose) yError(
"iDynChain: computeGeoJacobian() failed due to out of range indexes: from 0 to %d >= %d \n", iLinkN,
N);
1339 Matrix J(6, iLinkN+1 );J.zero();
1345 for (
unsigned int i=0; i<iLinkN; i++)
1346 intH.push_back(intH[i]*
allList[i]->getH(
true));
1348 for (
unsigned int i=0; i<iLinkN; i++)
1350 unsigned int j=
hash[i];
1352 w=
cross(Z,2,Pn-Z,3);
1367 if(
verbose)yError(
"iDynChain: computeGeoJacobian() failed since DOF==0 \n");
1372 if(
verbose) yError(
"iDynChain: computeGeoJacobian() failed due to out of range indexes: from 0 to %d >= %d \n",iLinkN,
N);
1377 Matrix J(6, iLinkN+1 );J.zero();
1382 intH.push_back(_H0);
1383 for (
unsigned int i=0; i<iLinkN; i++)
1384 intH.push_back(intH[i]*
allList[i]->getH(
true));
1386 for (
unsigned int i=0; i<iLinkN; i++)
1388 unsigned int j=
hash[i];
1390 w=
cross(Z,2,Pn-Z,3);
1405 if(
verbose) yError(
"iDynChain: computeGeoJacobian() failed since DOF==0 \n");
1410 Matrix J(6,
DOF); J.zero();
1416 for(
unsigned int i=0; i<
N; i++)
1417 intH.push_back(intH[i]*
allList[i]->getH(
true));
1419 for(
unsigned int i=0; i<
DOF; i++)
1421 unsigned int j=
hash[i];
1423 w=
cross(Z,2,Pn-Z,3);
1438 if(
verbose) yError(
"iDynChain: computeGeoJacobian() failed since DOF==0 \n");
1443 Matrix J(6,
DOF); J.zero();
1448 intH.push_back(_H0);
1449 for(
unsigned int i=0; i<
N; i++)
1450 intH.push_back(intH[i]*
allList[i]->getH(
true));
1452 for(
unsigned int i=0; i<
DOF; i++)
1454 unsigned int j=
hash[i];
1456 w=
cross(Z,2,Pn-Z,3);
1482 if(
verbose) yError(
"computeCOMJacobian() failed due to out of range index: %d >= %d \n",iLink,
N);
1488 Matrix J(6,iLink+1);
1496 for (
unsigned int j=0; j<=iLink; j++)
1497 intH.push_back(intH[j]*
allList[j]->getH(
true));
1499 Pn=intH[iLink+1]*
allList[iLink]->getCOM();
1501 for (
unsigned int j=0; j<=iLink; j++)
1504 w=
cross(Z,2,Pn-Z,3);
1521 if(
verbose) yError(
"computeCOMJacobian() failed due to out of range index: %d >= %d \n", iLink,
N);
1525 Matrix J(6,iLink+1);
1532 for (
unsigned int j=0; j<=iLink; j++)
1533 intH.push_back(intH[j]*
allList[j]->getH(
true));
1535 for (
unsigned int j=0; j<=iLink; j++)
1538 w=
cross(Z,2,Pn-Z,3);
1555 if(
verbose) yError(
"computeCOMJacobian() failed due to out of range index: %d >= %d \n", iLink,
N);
1559 Matrix J(6,iLink+1);
1564 intH.push_back(_H0);
1566 for (
unsigned int j=0; j<=iLink; j++)
1567 intH.push_back(intH[j]*
allList[j]->getH(
true));
1569 for (
unsigned int j=0; j<=iLink; j++)
1572 w=
cross(Z,2,Pn-Z,3);
1589 if(
verbose) yError(
"iDynChain: error, getCOM() failed due to out of range index: %d >= %d \n", iLink,
N);
1592 return allList[iLink]->getCOM();
1599 if(
verbose) yError(
"iDynChain: error, getHCOM() failed due to out of range index: %d >= %d \n", iLink,
N);
1602 return getH(iLink,
true) *
allList[iLink]->getCOM();
1618 for(
int i=
DOF-1; i>=0; i--)
1625 for(
unsigned int j=1+hash_i; j<
N+1; j++)
1629 for(
int j=
N; j>=hash_i; j--)
1633 for(
unsigned int j=i; j<
DOF; j++)
1682 for(
unsigned int i=0; i<
DOF; i++)
1697 Vector g(
DOF), zero3(3, 0.0);
1702 for(
unsigned int i=0; i<
DOF; i++)
1715 Vector ccg(
DOF), zero3(3, 0.0);
1722 for(
unsigned int i=0; i<
DOF; i++)
1777 type=option.check(
"type",Value(
"right")).asString().c_str();
1780 yError(
"Error: invalid handedness type specified! \n");
1785 if(Bottle *bH0=option.find(
"H0").asList())
1790 for(
int cnt=0; (cnt<bH0->size()) && (cnt<
H0.rows()*
H0.cols()); cnt++)
1792 H0(i,j)=bH0->get(cnt).asFloat64();
1802 int numLinks=option.check(
"numLinks",Value(0)).asInt32();
1805 yError(
"Error: invalid number of links (0) specified! \n");
1811 for(
int iLink=0; iLink<numLinks; iLink++)
1814 sprintf(link,
"link_%d",iLink);
1816 Bottle &bLink=option.findGroup(link);
1819 yError(
"Error: link %d is missing! \n",iLink);
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();
1836 double mass=bLink.check(
"mass",Value(0.0)).asFloat64();
1838 if(Bottle *bI=option.find(
"Inertia").asList())
1842 for(
int cnt=0; (cnt<bI->size()) && (cnt<I.rows()*I.cols()); cnt++)
1844 I(i,j)=bI->get(cnt).asFloat64();
1853 if(Bottle *bHC=option.find(
"H_COM").asList())
1857 for(
int cnt=0; (cnt<bHC->size()) && (cnt<HC.rows()*HC.cols()); cnt++)
1859 HC(i,j)=bHC->get(cnt).asFloat64();
1871 if(bLink.check(
"blocked"))
1914 for(
unsigned int i=0; i<limb.
getN(); i++)
1928 for(
unsigned int i=0; i<
linkList.size(); i++)
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.005
e-3, 18.7
e-3, 1.19
e-3, 123.0
e-6, 0.021
e-6, -0.001
e-6, 24.4
e-6, 4.22
e-6, 113.0
e-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.094
e-3, -6.27
e-3, -16.6
e-3, 137.0
e-6, -0.453
e-06, 0.203
e-06, 83.0
e-6, 20.7
e-6, 99.3
e-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.79
e-3, -62.9
e-3, 0.064
e-03, 743.0
e-6, 63.9
e-6, 0.851
e-06, 336.0
e-6, -3.61
e-6, 735.0
e-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.7
e-3, -3.71
e-3, 1.05
e-3, 28.4
e-6, -0.502
e-6, -0.399
e-6, 9.24
e-6, -0.371
e-6, 29.9
e-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.347
e-3, 71.3
e-3, -4.76
e-3, 766.0
e-6, 5.66
e-6, 1.40
e-6, 164.0
e-6, 18.2
e-6, 699.0
e-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.73
e-3, -8.05
e-3, -9.00
e-3, 154.0
e-6, 12.6
e-6, -6.08
e-6, 250.0
e-6, 17.6
e-6, 378.0
e-6, 0.0625, 0.016, 0.0,
M_PI, -20.0*
CTRL_DEG2RAD, 40.0*
CTRL_DEG2RAD));
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.004
e-3, 14.915
e-3, -0.019
e-3, 54.421
e-6, 0.009
e-6, 0.0
e-6, 9.331
e-6, -0.017
e-6, 54.862
e-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.097
e-3, -6.271
e-3, 16.622
e-3, 137.2
e-6, 0.466
e-6, 0.365
e-6, 82.927
e-6, -20.524
e-6, 99.274
e-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.769
e-3, 63.302
e-3, -0.084
e-3, 748.531
e-6, 63.340
e-6, -0.903
e-6, 338.109
e-6, -4.031
e-6, 741.022
e-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.718
e-3, 3.712
e-3, -1.046
e-3, 28.389
e-6, -0.515
e-6, -0.408
e-6, 9.244
e-6, -0.371
e-6, 29.968
e-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.264
e-3, -71.327
e-3, 4.672
e-3, 765.393
e-6, 4.337
e-6, 0.239
e-6, 164.578
e-6, 19.381
e-6, 698.060
e-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.851
e-3, -8.319
e-3, 9.284
e-3, 157.143
e-6, 12.780
e-6, 4.823
e-6, 247.995
e-6, -18.188
e-6, 380.535
e-6, 0.0625, -0.016, 0.0, 0.0, -20.0*
CTRL_DEG2RAD, 40.0*
CTRL_DEG2RAD));
2011 #ifdef NO_JOINT_COSTRAINTS
2021 IControlLimits &limTorso=*lim[0];
2022 IControlLimits &limArm =*lim[1];
2024 unsigned int iTorso;
2028 for (iTorso=0; iTorso<3; iTorso++)
2030 if (!limTorso.getLimits(iTorso,&
min,&
max))
2037 for (iArm=0; iArm<
getN()-iTorso; iArm++)
2039 if (!limArm.getLimits(iArm,&
min,&
max))
2090 pushLink(
new iDynLink(0.189*
mra, 0.005
e-3, 18.7
e-3, 1.19
e-3, 123.0
e-6, 0.021
e-6, -0.001
e-6, 24.4
e-6, 4.22
e-6, 113.0
e-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.094
e-3, -6.27
e-3, -16.6
e-3, 137.0
e-6, -0.453
e-06, 0.203
e-06, 83.0
e-6, 20.7
e-6, 99.3
e-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.79
e-3, -62.9
e-3, 0.064
e-03, 743.0
e-6, 63.9
e-6, 0.851
e-06, 336.0
e-6, -3.61
e-6, 735.0
e-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.7
e-3, -3.71
e-3, 1.05
e-3, 28.4
e-6, -0.502
e-6, -0.399
e-6, 9.24
e-6, -0.371
e-6, 29.9
e-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.347
e-3, 71.3
e-3, -4.76
e-3, 766.0
e-6, 5.66
e-6, 1.40
e-6, 164.0
e-6, 18.2
e-6, 699.0
e-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.73
e-3, -8.05
e-3, -9.00
e-3, 154.0
e-6, 12.6
e-6, -6.08
e-6, 250.0
e-6, 17.6
e-6, 378.0
e-6, 0.0625, 0.016, 0.0,
M_PI, -20.0*
CTRL_DEG2RAD, 40.0*
CTRL_DEG2RAD));
2103 pushLink(
new iDynLink(0.189*
mla, -0.005
e-3, 18.7
e-3, -1.19
e-3, 54.421
e-6, 0.009
e-6, 0.0
e-6, 9.331
e-6, -0.017
e-6, 54.862
e-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.094
e-3, -6.27
e-3, 16.6
e-3, 137.2
e-6, 0.466
e-6, 0.365
e-6, 82.927
e-6, -20.524
e-6, 99.274
e-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.79
e-3, 62.9
e-3, -0.064
e-3, 748.531
e-6, 63.340
e-6, -0.903
e-6, 338.109
e-6, -4.031
e-6, 741.022
e-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.7
e-3, 3.71
e-3, -1.05
e-3, 28.389
e-6, -0.515
e-6, -0.408
e-6, 9.244
e-6, -0.371
e-6, 29.968
e-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.347
e-3, -71.3
e-3, 4.76
e-3, 765.393
e-6, 4.337
e-6, 0.239
e-6, 164.578
e-6, 19.381
e-6, 698.060
e-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.73
e-3, -8.05
e-3, 9.00
e-3, 157.143
e-6, 12.780
e-6, 4.823
e-6, 247.995
e-6, -18.188
e-6, 380.535
e-6, 0.0625, -0.016, 0.0, 0.0, -20.0*
CTRL_DEG2RAD, 40.0*
CTRL_DEG2RAD));
2112 #ifdef NO_JOINT_COSTRAINTS
2122 IControlLimits &limArm =*lim[0];
2127 for (iArm=0; iArm<
getN(); iArm++)
2129 if (!limArm.getLimits(iArm,&
min,&
max))
2182 #ifdef TORSO_NO_WEIGHT
2183 pushLink(
new iDynLink(0.00
e-3, 0.000
e-3, 0.000
e-3, 0.000
e-3, 0.000
e-6, 0.000
e-6, 0.000
e-6, 0.000
e-6, 0.000
e-6, 0.000
e-6, 32.0
e-3, 0,
M_PI/2.0, 0.0, -100.0*
CTRL_DEG2RAD, 100.0*
CTRL_DEG2RAD));
2184 pushLink(
new iDynLink(0.00
e-3, 0.000
e-3, 0.000
e-3, 0.000
e-3, 0.000
e-6, 0.000
e-6, 0.000
e-6, 0.000
e-6, 0.000
e-6, 0.000
e-6, 0, -5.5
e-3,
M_PI/2.0, -
M_PI/2.0, -100.0*
CTRL_DEG2RAD, 100.0*
CTRL_DEG2RAD));
2185 pushLink(
new iDynLink(0.00
e-3, 0.000
e-3, 0.000
e-3, 0.000
e-3, 0.000
e-6, 0.000
e-6, 0.000
e-6, 0.000
e-6, 0.000
e-6, 0.000
e-6, 2.31
e-3, -193.3
e-3, -
M_PI/2.0, -
M_PI/2.0, -100.0*
CTRL_DEG2RAD, 100.0*
CTRL_DEG2RAD));
2192 pushLink(
new iDynLink(0, 3.120
e-2, 0, -9.758
e-7, 4.544
e-4, -4.263
e-5, -3.889
e-8, 1.141
e-3, 0.000
e-0, 1.236
e-3, 32.0
e-3, 0,
M_PI/2.0, 0.0, -100.0*
CTRL_DEG2RAD, 100.0*
CTRL_DEG2RAD));
2193 pushLink(
new iDynLink(0, 0, +4.296
e-5, -1.360
e-3, 5.308
e-4, -1.923
e-6, 5.095
e-5, 2.031
e-3, -3.849
e-7, 1.803
e-3, 0, -5.5
e-3,
M_PI/2.0, -
M_PI/2.0, -100.0*
CTRL_DEG2RAD, 100.0*
CTRL_DEG2RAD));
2194 pushLink(
new iDynLink(4.81
e+0, -8.102
e-5, -1.183
e-1, 0, 7.472
e-2, -3.600
e-6, -4.705
e-5, 8.145
e-2, 4.567
e-3, 1.306
e-2, 2.31
e-3, -193.3
e-3, -
M_PI/2.0, -
M_PI/2.0, -100.0*
CTRL_DEG2RAD, 100.0*
CTRL_DEG2RAD));
2197 #ifdef NO_JOINT_COSTRAINTS
2207 IControlLimits &limTorso=*lim[0];
2209 unsigned int iTorso;
2212 for (iTorso=0; iTorso<3; iTorso++)
2214 if (!limTorso.getLimits(iTorso,&
min,&
max))
2263 #ifdef LEGS_NO_WEIGHT
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));
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));
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));
2302 pushLink(
new iDynLink(0.754, 0, -0.0782, 0, 471.076
e-6, 2.059
e-6, 1.451
e-6, 346.478
e-6, 1.545
e-6, 510.315
e-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.0487
e-6, -0.074
e-6, -0.062
e-6, 561.583
e-6, 10.835
e-6, 294.119
e-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.073
e-6, -67.260
e-6, 2.267
e-6,1423.0245
e-6,36.37258
e-6,7553.849
e-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.950
e-6,-185.699
e-6,-63.147
e-6, 4450.537
e-6, 0.786
e-6,4207.657
e-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.230
e-6, -7.081
e-6, 41.421
e-6, 687.760
e-6, 20.817
e-6, 313.897
e-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));
2311 #ifdef NO_JOINT_COSTRAINTS
2321 IControlLimits &limLeg=*lim[0];
2326 for (iLeg=0; iLeg<
getN(); iLeg++)
2328 if (!limLeg.getLimits(iLeg,&
min,&
max))
2436 #ifdef LEGS_NO_WEIGHT
2442 pushLink(
new iDynLink(0, 80.4310
e-6, -6.91643
e-3, 2.30470
e-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.3229
e-6, -1.09640
e-3, 59.8624
e-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.1143
e-3, 60.0431
e-3, -1.3437
e-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.3956
e-3, -45.0091
e-6, -6.2408
e-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.3995
e-6, -0.858761
e-3, -10.2169
e-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.9310
e-6, 24.0690
e-3, 0, 0, 0, 0, 0, 0, -0.0685, 0.0035,
M_PI, 0.0, -24.0*
CTRL_DEG2RAD, 24.0*
CTRL_DEG2RAD));
2452 pushLink(
new iDynLink(0, 80.4310
e-6, -6.91643
e-3, -2.30470
e-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.3229
e-6, -1.09640
e-3, -59.8624
e-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.1143
e-3, 60.0431
e-3, 1.3437
e-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.3956
e-3, -45.0091
e-6, 6.2408
e-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.3995
e-6, -0.858761
e-3, 10.2169
e-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.9310
e-6, 24.0690
e-3, 0, 0, 0, 0, 0, 0, -0.0685, -0.0035, 0, 0.0, -24.0*
CTRL_DEG2RAD, 24.0*
CTRL_DEG2RAD));
2466 pushLink(
new iDynLink(0.695, 80.4310
e-6, -6.91643
e-3, 2.30470
e-3, 4470.534
e-6, -22.209
e-6, -13.9712
e-6, 3250.8524
e-6, -47.7629
e-6, 4904.8747
e-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.3229
e-6, -1.09640
e-3, 59.8624
e-3, 27271.4
e-6, -1.2116
e-6, 11.5713
e-6, 25364.6
e-6, -491.0806
e-6, 4173.9
e-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.1143
e-3, 60.0431
e-3, -1.3437
e-3, 45491.7
e-6, -940.9711
e-6, -53.7405
e-6, 10040.0
e-6, -1203.0
e-6, 45825.5
e-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.3956
e-3, -45.0091
e-6, -6.2408
e-3, 15455.9
e-6, -1390.3
e-6, 3945.4
e-6, 70277.0
e-6, -719.9501
e-6, 65964.5
e-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.5892
e-6, -0.82253
e-3, -10.2574
e-3, 4765.926
e-3, 9.7426
e-6, -26.8085
e-6, 4093.5
e-6, 353.7961
e-6, 3668.8
e-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.41539
e-3, 613.9310
e-6, 24.0690
e-3, 1584.66
e-6, 14.357686
e-6, 21.079452
e-6, 2014.6957
e-6, 17.241674
e-6, 977.22556
e-6, -0.0685, 0.0035,
M_PI, 0.0, -24.0*
CTRL_DEG2RAD, 24.0*
CTRL_DEG2RAD));
2477 pushLink(
new iDynLink(0.695, 80.4310
e-6, -6.91643
e-3, -2.30470
e-3, 4470.534
e-6, -22.209
e-6, 13.9712
e-6, 3250.8524
e-6, 47.7629
e-6, 4904.8747
e-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.3229
e-6, -1.09640
e-3, -59.8624
e-3, 27271.4
e-6, 1.2116
e-6, 11.5713
e-6, 25364.6
e-6, 491.0806
e-6, 4173.9
e-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.1143
e-3, 60.0431
e-3, 1.3437
e-3, 45491.7
e-6, -940.9711
e-6, 53.7405
e-6, 10040.0
e-6, 1203.0
e-6, 45825.5
e-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.3956
e-3, -45.0091
e-6, 6.2408
e-3, 15455.9
e-6, -1390.3
e-6, -3945.4
e-6, 70277.0
e-6, 719.9501
e-6, 65964.5
e-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.5892
e-6, -0.82253
e-3, 10.2574
e-3, 4765.926
e-3, 9.7426
e-6, 26.8085
e-6, 4093.5
e-6, -353.7961
e-6, 3668.8
e-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.4154
e-3, -613.9310
e-6, 24.0690
e-3, 1584.66
e-6, -14.357866
e-6, 21.079452
e-6, 2014.6957
e-6, -17.241674
e-6, 977.22556
e-6, -0.0685, -0.0035, 0, 0.0, -24.0*
CTRL_DEG2RAD, 24.0*
CTRL_DEG2RAD));
2487 #ifdef NO_JOINT_COSTRAINTS
2498 IControlLimits &limLeg=*lim[0];
2503 for (iLeg=0; iLeg<
getN(); iLeg++)
2505 if (!limLeg.getLimits(iLeg,&
min,&
max))
2540 pushLink(
new iDynLink(0.27017604, -30.535917
e-3, 0, -0.23571261
e-3, 100.46346
e-6, -0.17765781
e-6, 0.44914333
e-6, 45.425961
e-6, -0.12682862
e-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.3752947
e-3, 5.4544215
e-3, 142.82339
e-6, -0.0059261471
e-6, -0.0022006663
e-6, 82.884917
e-6, -9.1321119
e-6, 87.620338
e-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));
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));
2551 #ifdef NO_JOINT_COSTRAINTS
2561 IControlLimits &limHead =*lim[0];
2567 for (iHead=0; iHead<3; iHead++)
2569 if (!limHead.getLimits(iHead,&
min,&
max))
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));
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));
2630 #ifdef NO_JOINT_COSTRAINTS
2640 IControlLimits &limHead =*lim[0];
2646 for (iHead=0; iHead<3; iHead++)
2648 if (!limHead.getLimits(iHead,&
min,&
max))