private void IkCalculation(int legIndex)
{
if (legIndex == 5)
_lastLeg = true;
var gaitPosXyZrotY = Gait(legIndex, _travelRequest, _travelLengthX,
_travelLengthZ, _travelRotationY,
_gaitPosX, _gaitPosY, _gaitPosZ, _gaitRotY,
_numberOfLiftedPositions, _gaitLegNumber[legIndex],
_legLiftHeight, _liftDivisionFactor, _halfLiftHeight, _stepsInGait, _tlDivisionFactor);
_gaitPosX = gaitPosXyZrotY[0];
_gaitPosY = gaitPosXyZrotY[1];
_gaitPosZ = gaitPosXyZrotY[2];
_gaitRotY = gaitPosXyZrotY[3];
var angles = BodyLegIk(legIndex,
_legPosX[legIndex], _legPosY[legIndex], _legPosZ[legIndex],
_bodyPosX, _bodyPosY + LegYHeightCorrector[legIndex], _bodyPosZ,
_gaitPosX[legIndex], _gaitPosY[legIndex], _gaitPosZ[legIndex], _gaitRotY[legIndex],
_offsetX[legIndex], _offsetZ[legIndex],
_bodyRotX, _bodyRotZ, _bodyRotY, _calculatedCoxaAngle[legIndex]);
_coxaAngle[legIndex] = angles[0];
_femurAngle[legIndex] = angles[1];
_tibiaAngle[legIndex] = angles[2];
}