private void emergencyProcedure(bool forStartConfiguration)
{
setupInerpolators();
double normalizedTime = forStartConfiguration ? 0.01 : 0.99;
currentPosition = new Position(StartPositionX, StartPositionY, StartPositionZ);
currentRotation = new Rotation(startAngleR, startAngleP, startAngleY);
linearInterpolator.CalculateCurrentPosition(ref currentPosition, normalizedTime);
linearInterpolator.CalculateCurrentAngle(ref currentRotation, normalizedTime);
if (lerpActivated)
{
linearInterpolator.CalculateCurrentQuaternion(ref currentQuaternion, normalizedTime);
}
else
{
sphericalLinearInterpolator.CalculateCurrentQuaternion(ref currentQuaternion, normalizedTime);
}
robotLeft.SetupEffector(currentPosition, currentRotation);
robotLeft.calculateConfiguration(forStartConfiguration);
robotRight.SetupEffector(currentPosition, currentRotation);
robotRight.calculateConfiguration(forStartConfiguration);
}