Main Page | Namespace List | Class Hierarchy | Alphabetical List | Class List | File List | Namespace Members | Class Members | File Members | Related Pages

# Tools/Actorics/Kinematics.cpp

Go to the documentation of this file.

    00001 /**
    00002 * @file Kinematics.cpp
    00003 *
    00004 * Class Kinematics provides methods for robots kinematic calculations
    00005 *
    00006 * @author Max Risler
    00007 * @author Uwe Düffert
    00008 */
    00009
    00010 #include "Kinematics.h"
    00011 #include "Tools/RobotConfiguration.h"
    00012 #include "Tools/Math/Geometry.h"
    00013 #include "Tools/Math/Pose3D.h"
    00014 #include "Tools/Debugging/Debugging.h"
    00015
    00016
    00017 void Kinematics::getRelativeRobotVertices(RobotVertices& robotVertices, const JointData& jointData)
    00018 {
    00019     RobotVertices rob(getRobotConfiguration().getRobotDimensions()); //get shoulder positions from standard constructor
    00020
    00021     kneePositionFromJoints(fr,
    00022         fromMicroRad(jointData.data[JointData::legFR1]),
    00023         fromMicroRad(jointData.data[JointData::legFR2]),
    00024         rob.kneePosition[0].x, rob.kneePosition[0].y, rob.kneePosition[0].z);
    00025
    00026     kneePositionFromJoints(fl,
    00027         fromMicroRad(jointData.data[JointData::legFL1]),
    00028         fromMicroRad(jointData.data[JointData::legFL2]),
    00029         rob.kneePosition[1].x, rob.kneePosition[1].y, rob.kneePosition[1].z);
    00030
    00031     kneePositionFromJoints(hr,
    00032         fromMicroRad(jointData.data[JointData::legHR1]),
    00033         fromMicroRad(jointData.data[JointData::legHR2]),
    00034         rob.kneePosition[2].x, rob.kneePosition[2].y, rob.kneePosition[2].z);
    00035
    00036     kneePositionFromJoints(hl,
    00037         fromMicroRad(jointData.data[JointData::legHL1]),
    00038         fromMicroRad(jointData.data[JointData::legHL2]),
    00039         rob.kneePosition[3].x, rob.kneePosition[3].y, rob.kneePosition[3].z);
    00040
    00041
    00042     legPositionFromJoints(fr,
    00043         fromMicroRad(jointData.data[JointData::legFR1]),
    00044         fromMicroRad(jointData.data[JointData::legFR2]),
    00045         fromMicroRad(jointData.data[JointData::legFR3]),
    00046         rob.footPosition[0].x, rob.footPosition[0].y, rob.footPosition[0].z);
    00047
    00048     legPositionFromJoints(fl,
    00049         fromMicroRad(jointData.data[JointData::legFL1]),
    00050         fromMicroRad(jointData.data[JointData::legFL2]),
    00051         fromMicroRad(jointData.data[JointData::legFL3]),
    00052         rob.footPosition[1].x, rob.footPosition[1].y, rob.footPosition[1].z);
    00053
    00054     legPositionFromJoints(hr,
    00055         fromMicroRad(jointData.data[JointData::legHR1]),
    00056         fromMicroRad(jointData.data[JointData::legHR2]),
    00057         fromMicroRad(jointData.data[JointData::legHR3]),
    00058         rob.footPosition[2].x, rob.footPosition[2].y, rob.footPosition[2].z);
    00059
    00060     legPositionFromJoints(hl,
    00061         fromMicroRad(jointData.data[JointData::legHL1]),
    00062         fromMicroRad(jointData.data[JointData::legHL2]),
    00063         fromMicroRad(jointData.data[JointData::legHL3]),
    00064         rob.footPosition[3].x, rob.footPosition[3].y, rob.footPosition[3].z);
    00065
    00066     for (int i=0;i<4;i++)
    00067     {
    00068         rob.footPosition[i] += rob.shoulderPosition[i];
    00069         rob.kneePosition[i] += rob.shoulderPosition[i];
    00070     }
    00071     robotVertices = rob;
    00072 }
    00073
    00074 void Kinematics::getRobotTransformation(double& tilt,double& roll, const RobotVertices& relativeRobotVertices, const GroundMode mode, double expectedBodyTilt, double expectedBodyRoll)
    00075 {
    00076     switch (mode)
    00077     {
    00078     case constantMode:
    00079     case flyMode:
    00080         //assumption: user knows it better, eg from odometry.cfg
    00081         tilt=expectedBodyTilt;
    00082         roll=expectedBodyRoll;
    00083         break;
    00084     case dynamicMode:
    00085         {
    00086             //assumption: bodyRoll=0, feet or knees on ground
    00087             roll=0;
    00088             Vector2<double>front[4];
    00089             Vector2<double>hind[4];
    00090             int i;
    00091             double lowLowTilt;
    00092             //create a list of all relevant knee and foot points
    00093             for (i=0;i<2;i++)
    00094             {
    00095                 front[i]=Vector2<double>(relativeRobotVertices.footPosition[i].x,relativeRobotVertices.footPosition[i].z);
    00096                 front[i+2]=Vector2<double>(relativeRobotVertices.kneePosition[i].x,relativeRobotVertices.kneePosition[i].z);
    00097                 hind[i]=Vector2<double>(relativeRobotVertices.footPosition[i+2].x,relativeRobotVertices.footPosition[i+2].z);
    00098                 hind[i+2]=Vector2<double>(relativeRobotVertices.kneePosition[i+2].x,relativeRobotVertices.kneePosition[i+2].z);
    00099             }
    00100             int lowestFront,lowestHind;
    00101             for (lowestFront=0;lowestFront<4;lowestFront++)
    00102             {
    00103                 //find the lowest hind point for each front point
    00104                 lowestHind=0;
    00105                 for (int j=1;j<4;j++)
    00106                 {
    00107                     if ((front[lowestFront]-hind[j]).angle()>(front[lowestFront]-hind[lowestHind]).angle())
    00108                     {
    00109                         lowestHind=j;
    00110                     }
    00111                 }
    00112                 lowLowTilt=(front[lowestFront]-hind[lowestHind]).angle();
    00113                 //check whether the used front point is the lowestpoint for the found hind point
    00114                 if (((front[0]-hind[lowestHind]).angle()>lowLowTilt-0.0001)&&
    00115                     ((front[1]-hind[lowestHind]).angle()>lowLowTilt-0.0001)&&
    00116                     ((front[2]-hind[lowestHind]).angle()>lowLowTilt-0.0001)&&
    00117                     ((front[3]-hind[lowestHind]).angle()>lowLowTilt-0.0001))
    00118                 {
    00119                     //in this case we have found the pair of the lowest front and hind point
    00120                     break;
    00121                 }
    00122             }
    00123             tilt=lowLowTilt;
    00124             break;
    00125         }
    00126     case staticMode:
    00127         //assumption: feet on ground
    00128         /** @todo calulate lower feet diagonale calculate lower supporting
    00129         * feet triangle by checking on which side of the diagonale the center
    00130         * of gravity is
    00131         */
    00132         break;
    00133     case superMode:
    00134         //assumption: robots CG falls from expected bodyTilt/bodyRoll (e.g. given
    00135         //by sensor or previous call) to static position (side,front,hind,feet)
    00136         /** @todo thats quite tricky, but we may just check falling to
    00137         * side/front/hind and return staticMode solution otherwise
    00138         */
    00139         break;
    00140     default:
    00141         //OUTPUT(idText,text,"unknown GroundMode in Kinematics::getRobotTransformation()");
    00142         break;
    00143     }
    00144 }
    00145
    00146 void Kinematics::getAbsoluteRobotVertices(RobotVertices& robotVertices, const GroundMode mode, const JointData& jointData, double expectedBodyTilt, double expectedBodyRoll)
    00147 {
    00148     const RobotDimensions& r = getRobotConfiguration().getRobotDimensions();
    00149     RobotVertices relativeRobotVertices;
    00150     getRelativeRobotVertices(relativeRobotVertices, jointData);
    00151     double tilt,roll;
    00152     getRobotTransformation(tilt,roll, relativeRobotVertices, mode, expectedBodyTilt, expectedBodyRoll);
    00153     robotVertices.bodyTilt=tilt;
    00154     robotVertices.bodyRoll=roll;
    00155     robotVertices.neck=Vector3<double>(0,0,0);
    00156     int i;
    00157     for (i=0;i<4; i++)
    00158     {
    00159         {
    00160             robotVertices.footPosition[i].y=relativeRobotVertices.footPosition[i].y-relativeRobotVertices.neck.y;
    00161             Vector2<double>dist(relativeRobotVertices.footPosition[i].x-relativeRobotVertices.neck.x,relativeRobotVertices.footPosition[i].z-relativeRobotVertices.neck.z);
    00162             double arc=dist.angle()-(tilt-r.zeroBodyTilt);
    00163             robotVertices.footPosition[i].x=cos(arc)*dist.abs();
    00164             robotVertices.footPosition[i].z=sin(arc)*dist.abs();
    00165         }
    00166         {
    00167             robotVertices.kneePosition[i].y=relativeRobotVertices.kneePosition[i].y-relativeRobotVertices.neck.y;
    00168             Vector2<double>dist(relativeRobotVertices.kneePosition[i].x-relativeRobotVertices.neck.x,relativeRobotVertices.kneePosition[i].z-relativeRobotVertices.neck.z);
    00169             double arc=dist.angle()-(tilt-r.zeroBodyTilt);
    00170             robotVertices.kneePosition[i].x=cos(arc)*dist.abs();
    00171             robotVertices.kneePosition[i].z=sin(arc)*dist.abs();
    00172         }
    00173         {
    00174             robotVertices.shoulderPosition[i].y=relativeRobotVertices.shoulderPosition[i].y-relativeRobotVertices.neck.y;
    00175             Vector2<double>dist(relativeRobotVertices.shoulderPosition[i].x-relativeRobotVertices.neck.x,relativeRobotVertices.shoulderPosition[i].z-relativeRobotVertices.neck.z);
    00176             double arc=dist.angle()-(tilt-r.zeroBodyTilt);
    00177             robotVertices.shoulderPosition[i].x=cos(arc)*dist.abs();
    00178             robotVertices.shoulderPosition[i].z=sin(arc)*dist.abs();
    00179         }
    00180     }
    00181
    00182     //move the hovering robot to ground :-)
    00183     double minZ=0;
    00184     if (mode==flyMode)
    00185     {
    00186         minZ=-180;
    00187     }
    00188     else
    00189     {
    00190         for (i=0;i<4;i++)
    00191         {
    00192             if (robotVertices.footPosition[i].z<minZ) minZ=robotVertices.footPosition[i].z;
    00193             if (robotVertices.kneePosition[i].z<minZ) minZ=robotVertices.kneePosition[i].z;
    00194         }
    00195     }
    00196     for (i=0;i<4;i++)
    00197     {
    00198         robotVertices.footPosition[i].z -= minZ;
    00199         robotVertices.kneePosition[i].z -= minZ;
    00200         robotVertices.shoulderPosition[i].z -= minZ;
    00201     }
    00202     robotVertices.neck.z -= minZ;
    00203 }
    00204
    00205 void Kinematics::legPositionFromJoints(LegIndex leg, double j1, double j2, double j3, double &x, double &y, double &z, bool correctCalculation)
    00206 {
    00207     const RobotDimensions& r = getRobotConfiguration().getRobotDimensions();
    00208     double lowerleg;
    00209
    00210     if (correctCalculation) j1 += r.zeroShoulderArc; //this is because knee is not in the x-middle of a leg
    00211
    00212     if (FORELEG(leg))
    00213     {
    00214         if (correctCalculation) j3 -= r.zeroFrontKneeArc; //this is because knee is not in the x-middle of a leg
    00215         lowerleg=r.lowerForeLegLength;
    00216     }
    00217     else
    00218     {
    00219         if (correctCalculation) j3 -= r.zeroHindKneeArc; //this is because knee is not in the x-middle of a leg
    00220         lowerleg=r.lowerHindLegLength;
    00221     }
    00222
    00223     double s1=sin(j1),s2=sin(j2),s3=sin(j3);
    00224     double c1=cos(j1),c2=cos(j2),c3=cos(j3);
    00225
    00226
    00227     //calculate x,y relative to leg base
    00228     x=lowerleg*s1*c2*c3+lowerleg*c1*s3+r.upperLegLength*s1*c2;
    00229     y=lowerleg*s2*c3+r.upperLegLength*s2;
    00230
    00231     //calculate z
    00232     z=-lowerleg*c1*c2*c3+lowerleg*s1*s3-r.upperLegLength*c1*c2;
    00233
    00234     if (LEGBASE(leg)) return;
    00235
    00236     if (!FORELEG(leg))
    00237         x=-x;
    00238
    00239
    00240     if (!(LEFTLEG(leg)))
    00241     {
    00242         y=-y;
    00243     }
    00244 }
    00245
    00246 void Kinematics::kneePositionFromJoints(LegIndex leg, double j1, double j2, double &x, double &y, double &z, bool correctCalculation)
    00247 {
    00248     const RobotDimensions& r = getRobotConfiguration().getRobotDimensions();
    00249
    00250     if (correctCalculation) j1 += r.zeroShoulderArc; //this is because knee is not in the x-middle of a leg
    00251
    00252     double s1=sin(j1),s2=sin(j2);
    00253     double c1=cos(j1),c2=cos(j2);
    00254
    00255     //calculate x,y relative to leg base
    00256     x=r.upperLegLength*s1*c2;
    00257     y=r.upperLegLength*s2;
    00258
    00259     //calculate z
    00260     z=-r.upperLegLength*c1*c2;
    00261
    00262     if (LEGBASE(leg)) return;
    00263
    00264     if (!FORELEG(leg))
    00265         x=-x;
    00266
    00267     if (!(LEFTLEG(leg)))
    00268     {
    00269         y=-y;
    00270     }
    00271 }
    00272
    00273 void Kinematics::transformToLegBase(LegIndex leg,double &x, double &y, double &z, double &lowerleg) {
    00274     const RobotDimensions& r = getRobotConfiguration().getRobotDimensions();
    00275     if (FORELEG(leg))
    00276         lowerleg=r.lowerForeLegLength;
    00277     else
    00278         lowerleg=r.lowerHindLegLength;
    00279
    00280     if (LEGBASE(leg)) return;
    00281
    00282     //position relative to leg base
    00283     if (!FORELEG(leg))
    00284         x=-x;
    00285
    00286     if (LEFTLEG(leg))
    00287         y=y-r.bodyWidth/2;
    00288     else
    00289         y=-y-r.bodyWidth/2;
    00290 }
    00291
    00292 bool Kinematics::jointsFromLegPosition(LegIndex leg, double x, double y, double z, double &j1, double &j2, double &j3, double bodyTilt, bool correctCalculation)
    00293 {
    00294     const RobotDimensions& r = getRobotConfiguration().getRobotDimensions();
    00295     double a,b,c,d,c2;
    00296     double lowerleg;
    00297     double beta;
    00298
    00299     //double ox=x,oy=y,oz=z;
    00300
    00301     bool calcError=false;
    00302
    00303     transformToLegBase(leg,x,y,z,lowerleg);
    00304
    00305     //Beintransformation :
    00306     //(x,y,z,1) = Rot(y,-q1)*Rot(x,q2)*Trans(0,0,-u)*Rot(y,-q3)*Trans(0,0,-l)*(0,0,0,1)
    00307
    00308     // Winkel j3 ist der von einem Dreieck mit laengster Seite
    00309     // gleich der Laenge von Vektor (x,y,z)
    00310     // j3 mit Kosinussatz
    00311
    00312     c=x*x+y*y+z*z;
    00313
    00314     if (c>(lowerleg+r.upperLegLength)*(lowerleg+r.upperLegLength)) {
    00315         //OUTPUT(idText,"JointsFromLegPosition: Error "<<stringFromLeg(leg) <<" too far");
    00316         c=(lowerleg+r.upperLegLength)*(lowerleg+r.upperLegLength);
    00317         calcError=true;
    00318     }
    00319     if (c<(lowerleg-r.upperLegLength)*(lowerleg-r.upperLegLength)) {
    00320         //OUTPUT(idText,"JointsFromLegPosition: Error "<<stringFromLeg(leg) <<" too close");
    00321         c=(lowerleg-r.upperLegLength)*(lowerleg-r.upperLegLength);
    00322         calcError=true;
    00323     }
    00324
    00325     d=(c-lowerleg*lowerleg-r.upperLegLength*r.upperLegLength)/(2*lowerleg*r.upperLegLength);
    00326     j3=acos(d);
    00327
    00328     if (j3<r.jointLimitLeg3N) {
    00329         //OUTPUT(idText,"JointsFromLegPosition: Error "<<stringFromLeg(leg) <<" joint3");
    00330         j3=r.jointLimitLeg3N;
    00331         calcError=true;
    00332     } else if (j3>r.jointLimitLeg3P) {
    00333         //OUTPUT(idText,"JointsFromLegPosition: Error "<<stringFromLeg(leg) <<" joint3");
    00334         j3=r.jointLimitLeg3P;
    00335         calcError=true;
    00336     }
    00337
    00338     //mit j3 kann Teil der Transformation berechnet werden:
    00339     // Trans(0,0,-u)*Rot(y,-q3)*Trans(0,0,-l)*(0,0,0,1)=(a,0,b,1)
    00340
    00341     a=lowerleg*sin(j3);
    00342     b=-lowerleg*d-r.upperLegLength;
    00343
    00344     //Damit gilt (x,y,z,1) = Rot(y,-q1)*Rot(x,q2)*(a,0,b,1)
    00345     //    y = -b*sin(j2)
    00346
    00347     if (fabs(y)>fabs(b)) {
    00348         //OUTPUT(idText,"JointsFromLegPosition: Error "<<stringFromLeg(leg) <<" unreachable");
    00349         j2=pi_2;
    00350         calcError=true;
    00351     } else
    00352         j2=asin(-y/b);
    00353
    00354     if (j2<r.jointLimitLeg2N) {
    00355         //OUTPUT(idText,"JointsFromLegPosition: Error "<<stringFromLeg(leg) <<" joint2");
    00356         j2=r.jointLimitLeg2N;
    00357         calcError=true;
    00358     } else if (j2>r.jointLimitLeg2P) {
    00359         //OUTPUT(idText,"JointsFromLegPosition: Error "<<stringFromLeg(leg) <<" joint2");
    00360         j2=r.jointLimitLeg2P;
    00361         calcError=true;
    00362     }
    00363     c2=cos(j2);
    00364
    00365
    00366     //und x = a*cos(j1) - b*sin(j1)*cos(j2)
    00367
    00368     //mit a=d*cos(beta), b*c2=d*sin(beta),
    00369     //gilt x=d*cos(j2+beta)
    00370
    00371     if (fabs(c2)<0.0001) {
    00372         if (fabs(a)<0.0001)
    00373             j1=0; //Sonderfall Bein ausgestreckt j1 irrelevant
    00374         else
    00375             j1=acos(x/a);
    00376         if (z<0) j1=-j1;
    00377     } else {
    00378         beta=atan2(b*c2,a);
    00379         d=(b*c2)/sin(beta);
    00380         j1=acos(x/d);
    00381         if (z<0) j1=-j1;
    00382         j1-=beta;
    00383     }
    00384
    00385     if (correctCalculation) j1 -= r.zeroShoulderArc; //this is because knee is not in the x-middle of a leg
    00386
    00387     // reflect body tilt angle
    00388     if (FORELEG(leg))
    00389     {
    00390         j1 += bodyTilt;
    00391         if (correctCalculation) j3 += r.zeroFrontKneeArc; //this is because knee is not in the x-middle of a leg
    00392     }
    00393     else
    00394     {
    00395         j1 -= bodyTilt;
    00396         if (correctCalculation) j3 += r.zeroHindKneeArc; //this is because knee is not in the x-middle of a leg
    00397     }
    00398
    00399     if (j1<(FORELEG(leg)?r.jointLimitLeg1FN:r.jointLimitLeg1HN)) {
    00400         //OUTPUT(idText,"JointsFromLegPosition: Error "<<stringFromLeg(leg) <<" joint1");
    00401         j1=FORELEG(leg)?r.jointLimitLeg1FN:r.jointLimitLeg1HN;
    00402         calcError=true;
    00403     } else if (j1>(FORELEG(leg)?r.jointLimitLeg1FP:r.jointLimitLeg1HP)) {
    00404         //OUTPUT(idText,"JointsFromLegPosition: Error "<<stringFromLeg(leg) <<" joint1");
    00405         j1=FORELEG(leg)?r.jointLimitLeg1FP:r.jointLimitLeg1HP;
    00406         calcError=true;
    00407     }
    00408
    00409     /*
    00410     //test results
    00411     if (FORELEG(leg)) { j1 -= bodyTilt; } else { j1 += bodyTilt; }
    00412     double tx,ty,tz;
    00413     legPositionFromJoints(leg,j1,j2,j3,tx,ty,tz);
    00414     if (!(FORELEG(leg))) { tx = -tx; }
    00415     if (!(LEFTLEG(leg))) { ty = -ty; }
    00416     //OUTPUT(idText,text,"x: "<<tx<<" y: "<<ty<<" z: "<<tz);
    00417     if ((fabs(tx-x)>0.01 || fabs(ty-y)>0.01 || fabs(tz-z)>0.01) && !calcError)
    00418     OUTPUT(idText,text,"JointsFromLegPosition incorrect for "<<stringFromLeg(leg)<<" x: "<<x<<" y: "<<y<<" z: "<<z);
    00419     */
    00420     return !calcError;
    00421 }
    00422
    00423 char *Kinematics::stringFromLeg(LegIndex leg) {
    00424     switch(leg) {
    00425     case fr:
    00426         return "fore right leg";
    00427     case fl:
    00428         return "fore left leg";
    00429     case hr:
    00430         return "hind right leg";
    00431     case hl:
    00432         return "hind left leg";
    00433     case basefront:
    00434         return "front leg";
    00435     case basehind:
    00436         return "hind leg";
    00437     default:
    00438         return "wrong leg";
    00439     }
    00440 }
    00441
    00442 void Kinematics::calcRelativeRobotVertices(RobotVertices& rob, const SensorData& sensorData)
    00443 {
    00444     const RobotDimensions& r = getRobotConfiguration().getRobotDimensions();
    00445     long specsKneeArc = long(r.specsKneeArc * 1e6);
    00446     rob.shoulderPosition[fr] = Vector3<double>(-r.neckToLegsLengthX, -r.shoulderWidth / 2, -r.neckToLegsLengthZ);
    00447     Vector3<double> lower(r.lowerForeLegLengthX, 0, -r.lowerForeLegLengthZ);
    00448     lower *= (r.lowerForeLegLength - r.footRadius) / r.lowerForeLegLength;
    00449     calcLegPosition(rob.kneePosition[fr], rob.footPosition[fr],
    00450         -sensorData.data[SensorData::legFR1],
    00451         -sensorData.data[SensorData::legFR2], -sensorData.data[SensorData::legFR3] + specsKneeArc,
    00452         rob.shoulderPosition[fr], Vector3<double>(r.upperLegLengthX, -r.upperLegLengthY, -r.upperLegLengthZ),
    00453         lower);
    00454
    00455     rob.shoulderPosition[fl] = Vector3<double>(-r.neckToLegsLengthX, r.shoulderWidth / 2, -r.neckToLegsLengthZ);
    00456     calcLegPosition(rob.kneePosition[fl], rob.footPosition[fl],
    00457         -sensorData.data[SensorData::legFL1],
    00458         sensorData.data[SensorData::legFL2], -sensorData.data[SensorData::legFL3] + specsKneeArc,
    00459         rob.shoulderPosition[fl], Vector3<double>(r.upperLegLengthX, r.upperLegLengthY, -r.upperLegLengthZ),
    00460         lower);
    00461
    00462     lower = Vector3<double>(-r.lowerHindLegLengthX, 0, -r.lowerHindLegLengthZ);
    00463     lower *= (r.lowerHindLegLength - r.footRadius) / r.lowerHindLegLength;
    00464     rob.shoulderPosition[hr] = Vector3<double>(-r.neckToLegsLengthX - r.lengthBetweenLegs, -r.shoulderWidth / 2, -r.neckToLegsLengthZ);
    00465     calcLegPosition(rob.kneePosition[hr], rob.footPosition[hr],
    00466         sensorData.data[SensorData::legHR1],
    00467         -sensorData.data[SensorData::legHR2], sensorData.data[SensorData::legHR3] - specsKneeArc,
    00468         rob.shoulderPosition[hr], Vector3<double>(-r.upperLegLengthX, -r.upperLegLengthY, -r.upperLegLengthZ),
    00469         lower);
    00470
    00471     rob.shoulderPosition[hl] = Vector3<double>(-r.neckToLegsLengthX - r.lengthBetweenLegs, r.shoulderWidth / 2, -r.neckToLegsLengthZ);
    00472     calcLegPosition(rob.kneePosition[hl], rob.footPosition[hl],
    00473         sensorData.data[SensorData::legHL1],
    00474         sensorData.data[SensorData::legHL2], sensorData.data[SensorData::legHL3] - specsKneeArc,
    00475         rob.shoulderPosition[hl],
    00476         Vector3<double>(-r.upperLegLengthX, r.upperLegLengthY, -r.upperLegLengthZ),
    00477         lower);
    00478
    00479     for(int i = 0; i < 4; ++i)
    00480     {
    00481         rob.kneePosition[i].z -= r.kneeRadius;
    00482         rob.footPosition[i].z -= r.footRadius;
    00483     }
    00484     rob.frameNumber = sensorData.frameNumber;
    00485 }
    00486
    00487 void Kinematics::calcNeckAndLegPositions(const SensorData& sensorData, RobotVertices& rob)
    00488 {
    00489     const RobotDimensions& r = getRobotConfiguration().getRobotDimensions();
    00490
    00491     calcRelativeRobotVertices(rob, sensorData);
    00492
    00493     int i;
    00494     for(i = 0; i < 4; ++i)
    00495     {
    00496         if(rob.footPosition[i].z - rob.shoulderPosition[i].z > -r.shoulderRadius)
    00497             rob.footPosition[i] = rob.shoulderPosition[i] + Vector3<double>(0, 0, -r.shoulderRadius);
    00498         if(rob.kneePosition[i].z - rob.shoulderPosition[i].z > -r.shoulderRadius)
    00499             rob.kneePosition[i] = rob.shoulderPosition[i] + Vector3<double>(0, 0, -r.shoulderRadius);
    00500     }
    00501
    00502     getRobotTransformation(rob.bodyTilt, rob.bodyRoll, rob, Kinematics::dynamicMode);
    00503
    00504     Pose3D pose;
    00505     pose.rotateY(rob.bodyTilt);
    00506     double minZ = 0;
    00507     for(i = 0; i < 4; ++i)
    00508     {
    00509         rob.footPosition[i] = (Pose3D(pose).translate(rob.footPosition[i])).translation;
    00510         rob.kneePosition[i] = (Pose3D(pose).translate(rob.kneePosition[i])).translation;
    00511         if(minZ > rob.footPosition[i].z)
    00512             minZ = rob.footPosition[i].z;
    00513         if(minZ > rob.kneePosition[i].z)
    00514             minZ = rob.kneePosition[i].z;
    00515     }
    00516     rob.neckHeight = -minZ;
    00517 }
    00518
    00519 void Kinematics::calcLegPosition(Vector3<double>& kneePosition, Vector3<double>& footPosition,
    00520                                  long joint1, long joint2, long joint3,
    00521                                  const Vector3<double> shoulder,
    00522                                  const Vector3<double>& upper, const Vector3<double>& lower)
    00523 {
    00524     Pose3D pose = Pose3D(shoulder).rotateY(joint1 * 1e-6).rotateX(joint2 * 1e-6).translate(upper);
    00525     kneePosition = pose.translation;
    00526     footPosition = pose.rotateY(joint3 * 1e-6).translate(lower).translation;
    00527 }
    00528
    00529 void Kinematics::calculateCameraMatrix(const double tilt, const double pan, const double roll, const double bodyTilt, const double neckHeight, CameraMatrix& cameraMatrix)
    00530 {
    00531     const RobotConfiguration& rc = getRobotConfiguration();
    00532     const RobotDimensions& r = rc.getRobotDimensions();
    00533
    00534     /** @todo test if it is useful (faster) to expand these formulas: */
    00535     if(rc.getRobotDesign() == RobotDesign::ERS210)
    00536     {
    00537         (Pose3D&)cameraMatrix =
    00538         Pose3D(0, 0, neckHeight).
    00539         rotateX(rc.getRobotCalibration().bodyRollOffset).
    00540         rotateY(bodyTilt + rc.getRobotCalibration().bodyTiltOffset - tilt * rc.getRobotCalibration().tiltFactor).
    00541         translate(0, 0, r.distanceNeckToPanCenter).
    00542         rotateZ(pan * rc.getRobotCalibration().panFactor).
    00543         rotateY(rc.getRobotCalibration().headTiltOffset).
    00544         rotateX(roll + rc.getRobotCalibration().headRollOffset).
    00545         translate(r.distancePanCenterToCameraX, 0, 0);
    00546        }
    00547     else
    00548     {
    00549         (Pose3D&)cameraMatrix =
    00550         Pose3D(0, 0, neckHeight).
    00551         rotateX(rc.getRobotCalibration().bodyRollOffset).
    00552         rotateY(bodyTilt + rc.getRobotCalibration().bodyTiltOffset - tilt * rc.getRobotCalibration().tiltFactor).
    00553         translate(0, 0, r.distanceNeckToPanCenter).
    00554         rotateZ(pan * rc.getRobotCalibration().panFactor).
    00555         rotateY(-roll * rc.getRobotCalibration().tilt2Factor + rc.getRobotCalibration().headTiltOffset). // tilt2 joint
    00556         translate(r.distancePanCenterToCameraX, 0, -r.distancePanCenterToCameraZ).
    00557         rotateX(rc.getRobotCalibration().headRollOffset);
    00558        }
    00559 }
    00560
    00561 void Kinematics::calcHeadHeight(double& headHeight, const double bodyTilt, const double neckHeight, const SensorData& sensorData)
    00562 {
    00563     const RobotDimensions& rob = getRobotConfiguration().getRobotDimensions();
    00564
    00565     if (bodyTilt - fromMicroRad(sensorData.data[SensorData::neckTilt]) > pi/2)
    00566     {
    00567         headHeight = neckHeight - (cos(bodyTilt - fromMicroRad(sensorData.data[SensorData::neckTilt])) * rob.distanceNeckToPanCenter);
    00568     }else
    00569     {
    00570         headHeight = neckHeight + (cos(bodyTilt - fromMicroRad(sensorData.data[SensorData::neckTilt])) * rob.distanceNeckToPanCenter);
    00571     }
    00572 }
    00573
    00574 void Kinematics::calcNoseHeight(double& noseHeight, const double absHeadTilt, const double headHeight, const double bodyTilt, const double headRoll)
    00575 {
    00576     // @todo isn't noseHeight == cameraMatrix.translation.z ?
    00577     // Problem/difference is that this is able to calculate in advance - not measure
    00578     const RobotDimensions& rob = getRobotConfiguration().getRobotDimensions();
    00579
    00580     noseHeight = headHeight + (rob.distancePanCenterToCameraX * sin(absHeadTilt)) * sin(fabs(headRoll));
    00581 }
    00582
    00583
    00584 void Kinematics::calcAbsRoll(double& absRoll, const double bodyTilt, const SensorData& sensorData)
    00585 {
    00586     absRoll = - bodyTilt + fromMicroRad(sensorData.data[SensorData::neckTilt] + sensorData.data[SensorData::headTilt]);
    00587 }
    00588
    00589 double Kinematics::calcHighestPossibleTilt(const double absRoll, const double bodyTilt)
    00590 {
    00591     const RobotDimensions& rob = getRobotConfiguration().getRobotDimensions();
    00592
    00593     if (absRoll < rob.jointLimitHeadTiltN - bodyTilt)
    00594     {
    00595         return absRoll + bodyTilt - rob.jointLimitHeadTiltN;
    00596     }
    00597     else
    00598     {
    00599         return rob.jointLimitNeckTiltP;
    00600     }
    00601 }
    00602
    00603 double Kinematics::calcLowestPossibleTilt(const double absRoll, const double bodyTilt)
    00604 {
    00605     const RobotDimensions& rob = getRobotConfiguration().getRobotDimensions();
    00606
    00607     if (absRoll > rob.jointLimitHeadTiltP - bodyTilt + rob.jointLimitNeckTiltN)
    00608     {
    00609         return absRoll + bodyTilt - rob.jointLimitHeadTiltP;
    00610     }
    00611     else
    00612     {
    00613         return rob.jointLimitNeckTiltN;
    00614     }
    00615 }
    00616
    00617
    00618 void Kinematics::calcAbsRollJoints(double& neckTilt, double& absRoll, const double bodyTilt)
    00619 {
    00620     const RobotDimensions& rob = getRobotConfiguration().getRobotDimensions();
    00621     const double lowestTilt = Kinematics::calcLowestPossibleTilt(absRoll, bodyTilt);
    00622     const double highestTilt = Kinematics::calcHighestPossibleTilt(absRoll, bodyTilt);
    00623
    00624     if (neckTilt < lowestTilt)
    00625     {
    00626         neckTilt = lowestTilt;
    00627         absRoll = rob.jointLimitHeadTiltP;
    00628     }
    00629     else if (neckTilt > highestTilt)
    00630     {
    00631         neckTilt = highestTilt;
    00632         absRoll = rob.jointLimitHeadTiltN;
    00633     }
    00634     else
    00635     {
    00636         neckTilt = neckTilt;
    00637         absRoll += bodyTilt - neckTilt;
    00638     }
    00639 }