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); }