Tools/Actorics/Kinematics.cpp

Go to the documentation of this file.

/**
* @file Kinematics.cpp
*
* Class Kinematics provides methods for robots kinematic calculations
*
* @author Max Risler
* @author Uwe Düffert
*/

#include "Kinematics.h"
#include "Tools/RobotConfiguration.h"
#include "Tools/Math/Geometry.h"
#include "Tools/Math/Pose3D.h"
#include "Tools/Debugging/Debugging.h"


void Kinematics::getRelativeRobotVertices(RobotVertices& robotVertices, const JointData& jointData)
{
    RobotVertices rob(getRobotConfiguration().getRobotDimensions()); //get shoulder positions from standard constructor

    kneePositionFromJoints(fr,
        fromMicroRad(jointData.data[JointData::legFR1]),
        fromMicroRad(jointData.data[JointData::legFR2]),
        rob.kneePosition[0].x, rob.kneePosition[0].y, rob.kneePosition[0].z);

    kneePositionFromJoints(fl,
        fromMicroRad(jointData.data[JointData::legFL1]),
        fromMicroRad(jointData.data[JointData::legFL2]),
        rob.kneePosition[1].x, rob.kneePosition[1].y, rob.kneePosition[1].z);

    kneePositionFromJoints(hr,
        fromMicroRad(jointData.data[JointData::legHR1]),
        fromMicroRad(jointData.data[JointData::legHR2]),
        rob.kneePosition[2].x, rob.kneePosition[2].y, rob.kneePosition[2].z);

    kneePositionFromJoints(hl,
        fromMicroRad(jointData.data[JointData::legHL1]),
        fromMicroRad(jointData.data[JointData::legHL2]),
        rob.kneePosition[3].x, rob.kneePosition[3].y, rob.kneePosition[3].z);


    legPositionFromJoints(fr,
        fromMicroRad(jointData.data[JointData::legFR1]),
        fromMicroRad(jointData.data[JointData::legFR2]),
        fromMicroRad(jointData.data[JointData::legFR3]),
        rob.footPosition[0].x, rob.footPosition[0].y, rob.footPosition[0].z);

    legPositionFromJoints(fl,
        fromMicroRad(jointData.data[JointData::legFL1]),
        fromMicroRad(jointData.data[JointData::legFL2]),
        fromMicroRad(jointData.data[JointData::legFL3]),
        rob.footPosition[1].x, rob.footPosition[1].y, rob.footPosition[1].z);

    legPositionFromJoints(hr,
        fromMicroRad(jointData.data[JointData::legHR1]),
        fromMicroRad(jointData.data[JointData::legHR2]),
        fromMicroRad(jointData.data[JointData::legHR3]),
        rob.footPosition[2].x, rob.footPosition[2].y, rob.footPosition[2].z);

    legPositionFromJoints(hl,
        fromMicroRad(jointData.data[JointData::legHL1]),
        fromMicroRad(jointData.data[JointData::legHL2]),
        fromMicroRad(jointData.data[JointData::legHL3]),
        rob.footPosition[3].x, rob.footPosition[3].y, rob.footPosition[3].z);

    for (int i=0;i<4;i++)
    {
        rob.footPosition[i] += rob.shoulderPosition[i];
        rob.kneePosition[i] += rob.shoulderPosition[i];
    }
    robotVertices = rob;
}

void Kinematics::getRobotTransformation(double& tilt,double& roll, const RobotVertices& relativeRobotVertices, const GroundMode mode, double expectedBodyTilt, double expectedBodyRoll)
{
    switch (mode)
    {
    case constantMode:
    case flyMode:
        //assumption: user knows it better, eg from odometry.cfg
        tilt=expectedBodyTilt;
        roll=expectedBodyRoll;
        break;
    case dynamicMode:
        {
            //assumption: bodyRoll=0, feet or knees on ground
            roll=0;
            Vector2<double>front[4];
            Vector2<double>hind[4];
            int i;
            double lowLowTilt;
            //create a list of all relevant knee and foot points
            for (i=0;i<2;i++)
            {
                front[i]=Vector2<double>(relativeRobotVertices.footPosition[i].x,relativeRobotVertices.footPosition[i].z);
                front[i+2]=Vector2<double>(relativeRobotVertices.kneePosition[i].x,relativeRobotVertices.kneePosition[i].z);
                hind[i]=Vector2<double>(relativeRobotVertices.footPosition[i+2].x,relativeRobotVertices.footPosition[i+2].z);
                hind[i+2]=Vector2<double>(relativeRobotVertices.kneePosition[i+2].x,relativeRobotVertices.kneePosition[i+2].z);
            }
            int lowestFront,lowestHind;
            for (lowestFront=0;lowestFront<4;lowestFront++)
            {
                //find the lowest hind point for each front point
                lowestHind=0;
                for (int j=1;j<4;j++)
                {
                    if ((front[lowestFront]-hind[j]).angle()>(front[lowestFront]-hind[lowestHind]).angle())
                    {
                        lowestHind=j;
                    }
                }
                lowLowTilt=(front[lowestFront]-hind[lowestHind]).angle();
                //check whether the used front point is the lowestpoint for the found hind point
                if (((front[0]-hind[lowestHind]).angle()>lowLowTilt-0.0001)&&
                    ((front[1]-hind[lowestHind]).angle()>lowLowTilt-0.0001)&&
                    ((front[2]-hind[lowestHind]).angle()>lowLowTilt-0.0001)&&
                    ((front[3]-hind[lowestHind]).angle()>lowLowTilt-0.0001))
                {
                    //in this case we have found the pair of the lowest front and hind point
                    break;
                }
            }
            tilt=lowLowTilt;
            break;
        }
    case staticMode:
        //assumption: feet on ground
        /** @todo calulate lower feet diagonale calculate lower supporting
        * feet triangle by checking on which side of the diagonale the center
        * of gravity is
        */
        break;
    case superMode:
        //assumption: robots CG falls from expected bodyTilt/bodyRoll (e.g. given
        //by sensor or previous call) to static position (side,front,hind,feet)
        /** @todo thats quite tricky, but we may just check falling to
        * side/front/hind and return staticMode solution otherwise
        */
        break;
    default:
        //OUTPUT(idText,text,"unknown GroundMode in Kinematics::getRobotTransformation()");
        break;
    }
}

void Kinematics::getAbsoluteRobotVertices(RobotVertices& robotVertices, const GroundMode mode, const JointData& jointData, double expectedBodyTilt, double expectedBodyRoll)
{
    const RobotDimensions& r = getRobotConfiguration().getRobotDimensions();
    RobotVertices relativeRobotVertices;
    getRelativeRobotVertices(relativeRobotVertices, jointData);
    double tilt,roll;
    getRobotTransformation(tilt,roll, relativeRobotVertices, mode, expectedBodyTilt, expectedBodyRoll);
    robotVertices.bodyTilt=tilt;
    robotVertices.bodyRoll=roll;
    robotVertices.neck=Vector3<double>(0,0,0);
    int i;
    for (i=0;i<4; i++)
    {
        {
            robotVertices.footPosition[i].y=relativeRobotVertices.footPosition[i].y-relativeRobotVertices.neck.y;
            Vector2<double>dist(relativeRobotVertices.footPosition[i].x-relativeRobotVertices.neck.x,relativeRobotVertices.footPosition[i].z-relativeRobotVertices.neck.z);
            double arc=dist.angle()-(tilt-r.zeroBodyTilt);
            robotVertices.footPosition[i].x=cos(arc)*dist.abs();
            robotVertices.footPosition[i].z=sin(arc)*dist.abs();
        }
        {
            robotVertices.kneePosition[i].y=relativeRobotVertices.kneePosition[i].y-relativeRobotVertices.neck.y;
            Vector2<double>dist(relativeRobotVertices.kneePosition[i].x-relativeRobotVertices.neck.x,relativeRobotVertices.kneePosition[i].z-relativeRobotVertices.neck.z);
            double arc=dist.angle()-(tilt-r.zeroBodyTilt);
            robotVertices.kneePosition[i].x=cos(arc)*dist.abs();
            robotVertices.kneePosition[i].z=sin(arc)*dist.abs();
        }
        {
            robotVertices.shoulderPosition[i].y=relativeRobotVertices.shoulderPosition[i].y-relativeRobotVertices.neck.y;
            Vector2<double>dist(relativeRobotVertices.shoulderPosition[i].x-relativeRobotVertices.neck.x,relativeRobotVertices.shoulderPosition[i].z-relativeRobotVertices.neck.z);
            double arc=dist.angle()-(tilt-r.zeroBodyTilt);
            robotVertices.shoulderPosition[i].x=cos(arc)*dist.abs();
            robotVertices.shoulderPosition[i].z=sin(arc)*dist.abs();
        }
    }

    //move the hovering robot to ground :-)
    double minZ=0;
    if (mode==flyMode)
    {
        minZ=-180;
    }
    else
    {
        for (i=0;i<4;i++)
        {
            if (robotVertices.footPosition[i].z<minZ) minZ=robotVertices.footPosition[i].z;
            if (robotVertices.kneePosition[i].z<minZ) minZ=robotVertices.kneePosition[i].z;
        }
    }
    for (i=0;i<4;i++)
    {
        robotVertices.footPosition[i].z -= minZ;
        robotVertices.kneePosition[i].z -= minZ;
        robotVertices.shoulderPosition[i].z -= minZ;
    }
    robotVertices.neck.z -= minZ;
}

void Kinematics::legPositionFromJoints(LegIndex leg, double j1, double j2, double j3, double &x, double &y, double &z, bool correctCalculation)
{
    const RobotDimensions& r = getRobotConfiguration().getRobotDimensions();
    double lowerleg;

    if (correctCalculation) j1 += r.zeroShoulderArc; //this is because knee is not in the x-middle of a leg

    if (FORELEG(leg))
    {
        if (correctCalculation) j3 -= r.zeroFrontKneeArc; //this is because knee is not in the x-middle of a leg
        lowerleg=r.lowerForeLegLength;
    }
    else
    {
        if (correctCalculation) j3 -= r.zeroHindKneeArc; //this is because knee is not in the x-middle of a leg
        lowerleg=r.lowerHindLegLength;
    }

    double s1=sin(j1),s2=sin(j2),s3=sin(j3);
    double c1=cos(j1),c2=cos(j2),c3=cos(j3);


    //calculate x,y relative to leg base
    x=lowerleg*s1*c2*c3+lowerleg*c1*s3+r.upperLegLength*s1*c2;
    y=lowerleg*s2*c3+r.upperLegLength*s2;

    //calculate z
    z=-lowerleg*c1*c2*c3+lowerleg*s1*s3-r.upperLegLength*c1*c2;

    if (LEGBASE(leg)) return;

    if (!FORELEG(leg))
        x=-x;


    if (!(LEFTLEG(leg)))
    {
        y=-y;
    }
}

void Kinematics::kneePositionFromJoints(LegIndex leg, double j1, double j2, double &x, double &y, double &z, bool correctCalculation)
{
    const RobotDimensions& r = getRobotConfiguration().getRobotDimensions();

    if (correctCalculation) j1 += r.zeroShoulderArc; //this is because knee is not in the x-middle of a leg

    double s1=sin(j1),s2=sin(j2);
    double c1=cos(j1),c2=cos(j2);

    //calculate x,y relative to leg base
    x=r.upperLegLength*s1*c2;
    y=r.upperLegLength*s2;

    //calculate z
    z=-r.upperLegLength*c1*c2;

    if (LEGBASE(leg)) return;

    if (!FORELEG(leg))
        x=-x;

    if (!(LEFTLEG(leg)))
    {
        y=-y;
    }
}

void Kinematics::transformToLegBase(LegIndex leg,double &x, double &y, double &z, double &lowerleg) {
    const RobotDimensions& r = getRobotConfiguration().getRobotDimensions();
    if (FORELEG(leg))
        lowerleg=r.lowerForeLegLength;
    else
        lowerleg=r.lowerHindLegLength;

    if (LEGBASE(leg)) return;

    //position relative to leg base
    if (!FORELEG(leg))
        x=-x;

    if (LEFTLEG(leg))
        y=y-r.bodyWidth/2;
    else
        y=-y-r.bodyWidth/2;
}

bool Kinematics::jointsFromLegPosition(LegIndex leg, double x, double y, double z, double &j1, double &j2, double &j3, double bodyTilt, bool correctCalculation)
{
    const RobotDimensions& r = getRobotConfiguration().getRobotDimensions();
    double a,b,c,d,c2;
    double lowerleg;
    double beta;

    //double ox=x,oy=y,oz=z;

    bool calcError=false;

    transformToLegBase(leg,x,y,z,lowerleg);

    //Beintransformation :
    //(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)

    // Winkel j3 ist der von einem Dreieck mit laengster Seite 
    // gleich der Laenge von Vektor (x,y,z)
    // j3 mit Kosinussatz

    c=x*x+y*y+z*z;

    if (c>(lowerleg+r.upperLegLength)*(lowerleg+r.upperLegLength)) {
        //OUTPUT(idText,"JointsFromLegPosition: Error "<<stringFromLeg(leg) <<" too far");
        c=(lowerleg+r.upperLegLength)*(lowerleg+r.upperLegLength);
        calcError=true;
    }
    if (c<(lowerleg-r.upperLegLength)*(lowerleg-r.upperLegLength)) {
        //OUTPUT(idText,"JointsFromLegPosition: Error "<<stringFromLeg(leg) <<" too close");
        c=(lowerleg-r.upperLegLength)*(lowerleg-r.upperLegLength);
        calcError=true;
    }

    d=(c-lowerleg*lowerleg-r.upperLegLength*r.upperLegLength)/(2*lowerleg*r.upperLegLength);
    j3=acos(d);

    if (j3<r.jointLimitLeg3N) {
        //OUTPUT(idText,"JointsFromLegPosition: Error "<<stringFromLeg(leg) <<" joint3");
        j3=r.jointLimitLeg3N;
        calcError=true;
    } else if (j3>r.jointLimitLeg3P) {
        //OUTPUT(idText,"JointsFromLegPosition: Error "<<stringFromLeg(leg) <<" joint3");
        j3=r.jointLimitLeg3P;
        calcError=true;
    }

    //mit j3 kann Teil der Transformation berechnet werden:
    // Trans(0,0,-u)*Rot(y,-q3)*Trans(0,0,-l)*(0,0,0,1)=(a,0,b,1)

    a=lowerleg*sin(j3);
    b=-lowerleg*d-r.upperLegLength;

    //Damit gilt (x,y,z,1) = Rot(y,-q1)*Rot(x,q2)*(a,0,b,1)
    //    y = -b*sin(j2)

    if (fabs(y)>fabs(b)) {
        //OUTPUT(idText,"JointsFromLegPosition: Error "<<stringFromLeg(leg) <<" unreachable");
        j2=pi_2;
        calcError=true;
    } else
        j2=asin(-y/b);

    if (j2<r.jointLimitLeg2N) {
        //OUTPUT(idText,"JointsFromLegPosition: Error "<<stringFromLeg(leg) <<" joint2");
        j2=r.jointLimitLeg2N;
        calcError=true;
    } else if (j2>r.jointLimitLeg2P) {
        //OUTPUT(idText,"JointsFromLegPosition: Error "<<stringFromLeg(leg) <<" joint2");
        j2=r.jointLimitLeg2P;
        calcError=true;
    }
    c2=cos(j2);


    //und x = a*cos(j1) - b*sin(j1)*cos(j2)

    //mit a=d*cos(beta), b*c2=d*sin(beta),
    //gilt x=d*cos(j2+beta)

    if (fabs(c2)<0.0001) {
        if (fabs(a)<0.0001)
            j1=0; //Sonderfall Bein ausgestreckt j1 irrelevant
        else
            j1=acos(x/a);
        if (z<0) j1=-j1;
    } else {
        beta=atan2(b*c2,a);
        d=(b*c2)/sin(beta);
        j1=acos(x/d);
        if (z<0) j1=-j1;
        j1-=beta;
    } 

    if (correctCalculation) j1 -= r.zeroShoulderArc; //this is because knee is not in the x-middle of a leg

    // reflect body tilt angle
    if (FORELEG(leg))
    {
        j1 += bodyTilt;
        if (correctCalculation) j3 += r.zeroFrontKneeArc; //this is because knee is not in the x-middle of a leg
    }
    else
    {
        j1 -= bodyTilt;
        if (correctCalculation) j3 += r.zeroHindKneeArc; //this is because knee is not in the x-middle of a leg
    }

    if (j1<(FORELEG(leg)?r.jointLimitLeg1FN:r.jointLimitLeg1HN)) {
        //OUTPUT(idText,"JointsFromLegPosition: Error "<<stringFromLeg(leg) <<" joint1");
        j1=FORELEG(leg)?r.jointLimitLeg1FN:r.jointLimitLeg1HN;
        calcError=true;
    } else if (j1>(FORELEG(leg)?r.jointLimitLeg1FP:r.jointLimitLeg1HP)) {
        //OUTPUT(idText,"JointsFromLegPosition: Error "<<stringFromLeg(leg) <<" joint1");
        j1=FORELEG(leg)?r.jointLimitLeg1FP:r.jointLimitLeg1HP;
        calcError=true;
    }

    /*
    //test results
    if (FORELEG(leg)) { j1 -= bodyTilt; } else { j1 += bodyTilt; }
    double tx,ty,tz;
    legPositionFromJoints(leg,j1,j2,j3,tx,ty,tz);
    if (!(FORELEG(leg))) { tx = -tx; }
    if (!(LEFTLEG(leg))) { ty = -ty; }
    //OUTPUT(idText,text,"x: "<<tx<<" y: "<<ty<<" z: "<<tz);
    if ((fabs(tx-x)>0.01 || fabs(ty-y)>0.01 || fabs(tz-z)>0.01) && !calcError)
    OUTPUT(idText,text,"JointsFromLegPosition incorrect for "<<stringFromLeg(leg)<<" x: "<<x<<" y: "<<y<<" z: "<<z);
    */
    return !calcError;
}

char *Kinematics::stringFromLeg(LegIndex leg) {
    switch(leg) {
    case fr:
        return "fore right leg";
    case fl:
        return "fore left leg";
    case hr:
        return "hind right leg";
    case hl:
        return "hind left leg";
    case basefront:
        return "front leg";
    case basehind:
        return "hind leg";
    default:
        return "wrong leg";
    }
}

void Kinematics::calcRelativeRobotVertices(RobotVertices& rob, const SensorData& sensorData)
{
    const RobotDimensions& r = getRobotConfiguration().getRobotDimensions();
    long specsKneeArc = long(r.specsKneeArc * 1e6);
    rob.shoulderPosition[fr] = Vector3<double>(-r.neckToLegsLengthX, -r.shoulderWidth / 2, -r.neckToLegsLengthZ);
    Vector3<double> lower(r.lowerForeLegLengthX, 0, -r.lowerForeLegLengthZ);
    lower *= (r.lowerForeLegLength - r.footRadius) / r.lowerForeLegLength;
    calcLegPosition(rob.kneePosition[fr], rob.footPosition[fr], 
        -sensorData.data[SensorData::legFR1],
        -sensorData.data[SensorData::legFR2], -sensorData.data[SensorData::legFR3] + specsKneeArc,
        rob.shoulderPosition[fr], Vector3<double>(r.upperLegLengthX, -r.upperLegLengthY, -r.upperLegLengthZ),
        lower);

    rob.shoulderPosition[fl] = Vector3<double>(-r.neckToLegsLengthX, r.shoulderWidth / 2, -r.neckToLegsLengthZ);
    calcLegPosition(rob.kneePosition[fl], rob.footPosition[fl], 
        -sensorData.data[SensorData::legFL1],
        sensorData.data[SensorData::legFL2], -sensorData.data[SensorData::legFL3] + specsKneeArc,
        rob.shoulderPosition[fl], Vector3<double>(r.upperLegLengthX, r.upperLegLengthY, -r.upperLegLengthZ),
        lower);

    lower = Vector3<double>(-r.lowerHindLegLengthX, 0, -r.lowerHindLegLengthZ);
    lower *= (r.lowerHindLegLength - r.footRadius) / r.lowerHindLegLength;
    rob.shoulderPosition[hr] = Vector3<double>(-r.neckToLegsLengthX - r.lengthBetweenLegs, -r.shoulderWidth / 2, -r.neckToLegsLengthZ);
    calcLegPosition(rob.kneePosition[hr], rob.footPosition[hr], 
        sensorData.data[SensorData::legHR1],
        -sensorData.data[SensorData::legHR2], sensorData.data[SensorData::legHR3] - specsKneeArc,
        rob.shoulderPosition[hr], Vector3<double>(-r.upperLegLengthX, -r.upperLegLengthY, -r.upperLegLengthZ),
        lower);

    rob.shoulderPosition[hl] = Vector3<double>(-r.neckToLegsLengthX - r.lengthBetweenLegs, r.shoulderWidth / 2, -r.neckToLegsLengthZ);
    calcLegPosition(rob.kneePosition[hl], rob.footPosition[hl], 
        sensorData.data[SensorData::legHL1], 
        sensorData.data[SensorData::legHL2], sensorData.data[SensorData::legHL3] - specsKneeArc,
        rob.shoulderPosition[hl],
        Vector3<double>(-r.upperLegLengthX, r.upperLegLengthY, -r.upperLegLengthZ),
        lower);

    for(int i = 0; i < 4; ++i)
    {
        rob.kneePosition[i].z -= r.kneeRadius;
        rob.footPosition[i].z -= r.footRadius;
    }
    rob.frameNumber = sensorData.frameNumber;
}

void Kinematics::calcNeckAndLegPositions(const SensorData& sensorData, RobotVertices& rob)
{
    const RobotDimensions& r = getRobotConfiguration().getRobotDimensions();

    calcRelativeRobotVertices(rob, sensorData);

    int i;
    for(i = 0; i < 4; ++i)
    {
        if(rob.footPosition[i].z - rob.shoulderPosition[i].z > -r.shoulderRadius)
            rob.footPosition[i] = rob.shoulderPosition[i] + Vector3<double>(0, 0, -r.shoulderRadius);
        if(rob.kneePosition[i].z - rob.shoulderPosition[i].z > -r.shoulderRadius)
            rob.kneePosition[i] = rob.shoulderPosition[i] + Vector3<double>(0, 0, -r.shoulderRadius);
    }

    getRobotTransformation(rob.bodyTilt, rob.bodyRoll, rob, Kinematics::dynamicMode);

    Pose3D pose;
    pose.rotateY(rob.bodyTilt);
    double minZ = 0;
    for(i = 0; i < 4; ++i)
    {
        rob.footPosition[i] = (Pose3D(pose).translate(rob.footPosition[i])).translation;
        rob.kneePosition[i] = (Pose3D(pose).translate(rob.kneePosition[i])).translation;
        if(minZ > rob.footPosition[i].z)
            minZ = rob.footPosition[i].z;
        if(minZ > rob.kneePosition[i].z)
            minZ = rob.kneePosition[i].z;
    }
    rob.neckHeight = -minZ;
}

void Kinematics::calcLegPosition(Vector3<double>& kneePosition, Vector3<double>& footPosition,
                                 long joint1, long joint2, long joint3,
                                 const Vector3<double> shoulder,
                                 const Vector3<double>& upper, const Vector3<double>& lower)
{
    Pose3D pose = Pose3D(shoulder).rotateY(joint1 * 1e-6).rotateX(joint2 * 1e-6).translate(upper);
    kneePosition = pose.translation;
    footPosition = pose.rotateY(joint3 * 1e-6).translate(lower).translation;
}

void Kinematics::calculateCameraMatrix(const double tilt, const double pan, const double roll, const double bodyTilt, const double neckHeight, CameraMatrix& cameraMatrix)
{
    const RobotConfiguration& rc = getRobotConfiguration();
    const RobotDimensions& r = rc.getRobotDimensions();

    /** @todo test if it is useful (faster) to expand these formulas: */
    if(rc.getRobotDesign() == RobotDesign::ERS210)
    {
        (Pose3D&)cameraMatrix =
        Pose3D(0, 0, neckHeight).
        rotateX(rc.getRobotCalibration().bodyRollOffset).
        rotateY(bodyTilt + rc.getRobotCalibration().bodyTiltOffset - tilt * rc.getRobotCalibration().tiltFactor).
        translate(0, 0, r.distanceNeckToPanCenter).
        rotateZ(pan * rc.getRobotCalibration().panFactor).
        rotateY(rc.getRobotCalibration().headTiltOffset).
        rotateX(roll + rc.getRobotCalibration().headRollOffset).
        translate(r.distancePanCenterToCameraX, 0, 0);
       }
    else
    {
        (Pose3D&)cameraMatrix =
        Pose3D(0, 0, neckHeight).
        rotateX(rc.getRobotCalibration().bodyRollOffset).
        rotateY(bodyTilt + rc.getRobotCalibration().bodyTiltOffset - tilt * rc.getRobotCalibration().tiltFactor).
        translate(0, 0, r.distanceNeckToPanCenter).
        rotateZ(pan * rc.getRobotCalibration().panFactor).
        rotateY(-roll * rc.getRobotCalibration().tilt2Factor + rc.getRobotCalibration().headTiltOffset). // tilt2 joint
        translate(r.distancePanCenterToCameraX, 0, -r.distancePanCenterToCameraZ).
        rotateX(rc.getRobotCalibration().headRollOffset);
       }
}

void Kinematics::calcHeadHeight(double& headHeight, const double bodyTilt, const double neckHeight, const SensorData& sensorData)
{
    const RobotDimensions& rob = getRobotConfiguration().getRobotDimensions();

    if (bodyTilt - fromMicroRad(sensorData.data[SensorData::neckTilt]) > pi/2)
    {
        headHeight = neckHeight - (cos(bodyTilt - fromMicroRad(sensorData.data[SensorData::neckTilt])) * rob.distanceNeckToPanCenter);
    }else
    {
        headHeight = neckHeight + (cos(bodyTilt - fromMicroRad(sensorData.data[SensorData::neckTilt])) * rob.distanceNeckToPanCenter);
    }
}

void Kinematics::calcNoseHeight(double& noseHeight, const double absHeadTilt, const double headHeight, const double bodyTilt, const double headRoll)
{
    // @todo isn't noseHeight == cameraMatrix.translation.z ?
    // Problem/difference is that this is able to calculate in advance - not measure
    const RobotDimensions& rob = getRobotConfiguration().getRobotDimensions();

    noseHeight = headHeight + (rob.distancePanCenterToCameraX * sin(absHeadTilt)) * sin(fabs(headRoll));
}


void Kinematics::calcAbsRoll(double& absRoll, const double bodyTilt, const SensorData& sensorData)
{
    absRoll = - bodyTilt + fromMicroRad(sensorData.data[SensorData::neckTilt] + sensorData.data[SensorData::headTilt]);
}

double Kinematics::calcHighestPossibleTilt(const double absRoll, const double bodyTilt)
{
    const RobotDimensions& rob = getRobotConfiguration().getRobotDimensions();

    if (absRoll < rob.jointLimitHeadTiltN - bodyTilt)
    {
        return absRoll + bodyTilt - rob.jointLimitHeadTiltN;
    }
    else
    {
        return rob.jointLimitNeckTiltP;
    }
}

double Kinematics::calcLowestPossibleTilt(const double absRoll, const double bodyTilt)
{
    const RobotDimensions& rob = getRobotConfiguration().getRobotDimensions();

    if (absRoll > rob.jointLimitHeadTiltP - bodyTilt + rob.jointLimitNeckTiltN)
    {
        return absRoll + bodyTilt - rob.jointLimitHeadTiltP;
    }
    else
    {
        return rob.jointLimitNeckTiltN;
    }
}


void Kinematics::calcAbsRollJoints(double& neckTilt, double& absRoll, const double bodyTilt)
{
    const RobotDimensions& rob = getRobotConfiguration().getRobotDimensions();
    const double lowestTilt = Kinematics::calcLowestPossibleTilt(absRoll, bodyTilt);
    const double highestTilt = Kinematics::calcHighestPossibleTilt(absRoll, bodyTilt);

    if (neckTilt < lowestTilt)
    {
        neckTilt = lowestTilt;
        absRoll = rob.jointLimitHeadTiltP;
    }
    else if (neckTilt > highestTilt)
    {
        neckTilt = highestTilt;
        absRoll = rob.jointLimitHeadTiltN;
    }
    else
    {
        neckTilt = neckTilt;
        absRoll += bodyTilt - neckTilt;
    }
}

---