refactoring code, adjusting init values
start fixing issues in Euler angles. Problem left go out of domain for angles