Skip to content

Commit c1c49c1

Browse files
martinaxgloriamartinaxgloria
authored
[iKin] - Introduce iCub v2.10 (#1043)
1 parent 6643803 commit c1c49c1

File tree

2 files changed

+53
-8
lines changed

2 files changed

+53
-8
lines changed

‎src/libraries/iKin/src/iKinFwd.cpp‎

Lines changed: 53 additions & 7 deletions
Original file line numberDiff line numberDiff line change
@@ -2586,6 +2586,17 @@ void iCubEye::allocate(const string &_type)
25862586
pushLink(new iKinLink( 0.0, 0.034, -M_PI/2.0, 0.0, -35.0*CTRL_DEG2RAD, 15.0*CTRL_DEG2RAD));
25872587
pushLink(new iKinLink( 0.0, 0.0, M_PI/2.0, -M_PI/2.0, -50.0*CTRL_DEG2RAD, 50.0*CTRL_DEG2RAD));
25882588
}
2589+
else if (version==iKinLimbVersion("2.10"))
2590+
{
2591+
pushLink(new iKinLink( 0.032, 0.0, M_PI/2.0, 0.0, -22.0*CTRL_DEG2RAD, 84.0*CTRL_DEG2RAD));
2592+
pushLink(new iKinLink( 0.0, -0.0055, M_PI/2.0, -M_PI/2.0, -39.0*CTRL_DEG2RAD, 39.0*CTRL_DEG2RAD));
2593+
pushLink(new iKinLink( 0.0053, -0.2233, -M_PI/2.0, -M_PI/2.0, -59.0*CTRL_DEG2RAD, 59.0*CTRL_DEG2RAD));
2594+
pushLink(new iKinLink( 0.0110, 0.0, M_PI/2.0, M_PI/2.0, -40.0*CTRL_DEG2RAD, 30.0*CTRL_DEG2RAD));
2595+
pushLink(new iKinLink( 0.0, 0.0, -M_PI/2.0, -M_PI/2.0, -70.0*CTRL_DEG2RAD, 60.0*CTRL_DEG2RAD));
2596+
pushLink(new iKinLink(-0.0510, 0.08052, -M_PI/2.0, M_PI/2.0, -55.0*CTRL_DEG2RAD, 55.0*CTRL_DEG2RAD));
2597+
pushLink(new iKinLink( 0.0, 0.034, -M_PI/2.0, 0.0, -35.0*CTRL_DEG2RAD, 15.0*CTRL_DEG2RAD));
2598+
pushLink(new iKinLink( 0.0, 0.0, M_PI/2.0, -M_PI/2.0, -50.0*CTRL_DEG2RAD, 50.0*CTRL_DEG2RAD));
2599+
}
25892600
else if (version<iKinLimbVersion("3.0"))
25902601
{
25912602
pushLink(new iKinLink( 0.032, 0.0, M_PI/2.0, 0.0, -22.0*CTRL_DEG2RAD, 84.0*CTRL_DEG2RAD));
@@ -2597,7 +2608,7 @@ void iCubEye::allocate(const string &_type)
25972608
pushLink(new iKinLink( 0.0, 0.034, -M_PI/2.0, 0.0, -35.0*CTRL_DEG2RAD, 15.0*CTRL_DEG2RAD));
25982609
pushLink(new iKinLink( 0.0, 0.0, M_PI/2.0, -M_PI/2.0, -50.0*CTRL_DEG2RAD, 50.0*CTRL_DEG2RAD));
25992610
}
2600-
else // version>=3.0
2611+
else // version=3.0
26012612
{
26022613
H0.zero();
26032614
H0(0,2)=1.0;
@@ -2629,6 +2640,17 @@ void iCubEye::allocate(const string &_type)
26292640
pushLink(new iKinLink( 0.0, -0.034, -M_PI/2.0, 0.0, -35.0*CTRL_DEG2RAD, 15.0*CTRL_DEG2RAD));
26302641
pushLink(new iKinLink( 0.0, 0.0, M_PI/2.0, -M_PI/2.0, -50.0*CTRL_DEG2RAD, 50.0*CTRL_DEG2RAD));
26312642
}
2643+
else if (version==iKinLimbVersion("2.10"))
2644+
{
2645+
pushLink(new iKinLink( 0.032, 0.0, M_PI/2.0, 0.0, -22.0*CTRL_DEG2RAD, 84.0*CTRL_DEG2RAD));
2646+
pushLink(new iKinLink( 0.0, -0.0055, M_PI/2.0, -M_PI/2.0, -39.0*CTRL_DEG2RAD, 39.0*CTRL_DEG2RAD));
2647+
pushLink(new iKinLink( 0.0053, -0.2233, -M_PI/2.0, -M_PI/2.0, -59.0*CTRL_DEG2RAD, 59.0*CTRL_DEG2RAD));
2648+
pushLink(new iKinLink( 0.0110, 0.0, M_PI/2.0, M_PI/2.0, -40.0*CTRL_DEG2RAD, 30.0*CTRL_DEG2RAD));
2649+
pushLink(new iKinLink( 0.0, 0.0, -M_PI/2.0, -M_PI/2.0, -70.0*CTRL_DEG2RAD, 60.0*CTRL_DEG2RAD));
2650+
pushLink(new iKinLink(-0.0510, 0.08052, -M_PI/2.0, M_PI/2.0, -55.0*CTRL_DEG2RAD, 55.0*CTRL_DEG2RAD));
2651+
pushLink(new iKinLink( 0.0, -0.034, -M_PI/2.0, 0.0, -35.0*CTRL_DEG2RAD, 15.0*CTRL_DEG2RAD));
2652+
pushLink(new iKinLink( 0.0, 0.0, M_PI/2.0, -M_PI/2.0, -50.0*CTRL_DEG2RAD, 50.0*CTRL_DEG2RAD));
2653+
}
26322654
else if (version<iKinLimbVersion("3.0"))
26332655
{
26342656
pushLink(new iKinLink( 0.032, 0.0, M_PI/2.0, 0.0, -22.0*CTRL_DEG2RAD, 84.0*CTRL_DEG2RAD));
@@ -2640,7 +2662,7 @@ void iCubEye::allocate(const string &_type)
26402662
pushLink(new iKinLink( 0.0, -0.034, -M_PI/2.0, 0.0, -35.0*CTRL_DEG2RAD, 15.0*CTRL_DEG2RAD));
26412663
pushLink(new iKinLink( 0.0, 0.0, M_PI/2.0, -M_PI/2.0, -50.0*CTRL_DEG2RAD, 50.0*CTRL_DEG2RAD));
26422664
}
2643-
else // version>=3.0
2665+
else // version=3.0
26442666
{
26452667
H0.zero();
26462668
H0(0,2)=1.0;
@@ -2668,11 +2690,10 @@ void iCubEye::allocate(const string &_type)
26682690
setHN(HN);
26692691
}
26702692
// 4k cameras image plane
2671-
else if ((version==iKinLimbVersion("2.10")) ||
2672-
(version==iKinLimbVersion("3.1")))
2693+
else if (version==iKinLimbVersion("2.10"))
26732694
{
26742695
Matrix HN=yarp::math::eye(4, 4);
2675-
HN(2, 3)=-12.63e-3;
2696+
HN(2, 3)=-9.83e-3;
26762697
setHN(HN);
26772698
}
26782699

@@ -2820,6 +2841,19 @@ void iCubInertialSensor::allocate(const string &_type)
28202841
pushLink(new iKinLink( 0.0, 0.001, -M_PI/2.0, -M_PI/2.0, -70.0*CTRL_DEG2RAD, 60.0*CTRL_DEG2RAD));
28212842
pushLink(new iKinLink( 0.0225, 0.1005, -M_PI/2.0, M_PI/2.0, -55.0*CTRL_DEG2RAD, 55.0*CTRL_DEG2RAD));
28222843
}
2844+
else if (version==iKinLimbVersion("2.10"))
2845+
{
2846+
H0(0,1)=-1.0;
2847+
H0(1,2)=-1.0;
2848+
H0(2,0)=1.0;
2849+
2850+
pushLink(new iKinLink( 0.032, 0.0, M_PI/2.0, 0.0, -22.0*CTRL_DEG2RAD, 84.0*CTRL_DEG2RAD));
2851+
pushLink(new iKinLink( 0.0, -0.0055, M_PI/2.0, -M_PI/2.0, -39.0*CTRL_DEG2RAD, 39.0*CTRL_DEG2RAD));
2852+
pushLink(new iKinLink( 0.0, -0.2233, -M_PI/2.0, -M_PI/2.0, -59.0*CTRL_DEG2RAD, 59.0*CTRL_DEG2RAD));
2853+
pushLink(new iKinLink( 0.0110, 0.0, M_PI/2.0, M_PI/2.0, -40.0*CTRL_DEG2RAD, 30.0*CTRL_DEG2RAD));
2854+
pushLink(new iKinLink( 0.0, 0.0, -M_PI/2.0, -M_PI/2.0, -70.0*CTRL_DEG2RAD, 60.0*CTRL_DEG2RAD));
2855+
pushLink(new iKinLink( 0.0055, 0.1690, M_PI/2.0, 0.0, -55.0*CTRL_DEG2RAD, 55.0*CTRL_DEG2RAD));
2856+
}
28232857
else if (version<iKinLimbVersion("3.0"))
28242858
{
28252859
H0(0,1)=-1.0;
@@ -2833,7 +2867,7 @@ void iCubInertialSensor::allocate(const string &_type)
28332867
pushLink(new iKinLink( 0.0, 0.0, -M_PI/2.0, -M_PI/2.0, -70.0*CTRL_DEG2RAD, 60.0*CTRL_DEG2RAD));
28342868
pushLink(new iKinLink( 0.0185, 0.1108, -M_PI/2.0, M_PI/2.0, -55.0*CTRL_DEG2RAD, 55.0*CTRL_DEG2RAD));
28352869
}
2836-
else // version>=3.0
2870+
else // version=3.0
28372871
{
28382872
H0(0,2)=1.0;
28392873
H0(1,1)=-1.0;
@@ -2869,7 +2903,7 @@ void iCubInertialSensor::allocate(const string &_type)
28692903
}
28702904

28712905
// further displacement for >= 2.6
2872-
if (version>=iKinLimbVersion("2.6"))
2906+
if (version>=iKinLimbVersion("2.6") && version!=iKinLimbVersion("2.10"))
28732907
{
28742908
Matrix HN=zeros(4,4);
28752909
HN(0,3)=0.0323779;
@@ -2881,6 +2915,18 @@ void iCubInertialSensor::allocate(const string &_type)
28812915
HN(3,3)=1.0;
28822916
setHN(getHN()*HN);
28832917
}
2918+
2919+
// new HN for version 2.10 (see https://github.com/icub-tech-iit/study-icub-headedge/issues/230#issuecomment-3279162879)
2920+
else if (version==iKinLimbVersion("2.10"))
2921+
{
2922+
Matrix HN=zeros(4,4);
2923+
HN(0,0)=-1.0;
2924+
HN(1,2)=-1.0;
2925+
HN(2,1)=-1.0;
2926+
HN(2,3)=0.00345;
2927+
HN(3,3)=1.0;
2928+
setHN(HN);
2929+
}
28842930
}
28852931

28862932

‎src/modules/iKinGazeCtrl/src/main.cpp‎

Lines changed: 0 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -1074,7 +1074,6 @@ class GazeModule: public RFModule
10741074
d[iKinLimbVersion("2.8")-ver_in]=iKinLimbVersion("2.8");
10751075
d[iKinLimbVersion("2.10")-ver_in]=iKinLimbVersion("2.10");
10761076
d[iKinLimbVersion("3.0")-ver_in]=iKinLimbVersion("3.0");
1077-
d[iKinLimbVersion("3.1")-ver_in]=iKinLimbVersion("3.1");
10781077

10791078
auto ver_out=d.begin()->second;
10801079
if (ver_out!=ver_in)

0 commit comments

Comments
 (0)